added parametrisation of watchdog duration time

added parametrisation of topic for subscription name
SoftServeSAG · Mar 6, 2019 · 0615e30 · 0615e30
1 parent 44a537f
commit 0615e30
Show file tree

Hide file tree

Showing 3 changed files with 43 additions and 29 deletions.
diff --git a/cmd_vel_watchdog/launch/cmd_vel_watchdog.launch b/cmd_vel_watchdog/launch/cmd_vel_watchdog.launch
@@ -1,8 +1,11 @@
 <launch>
-    <node pkg="cmd_vel_watchdog" type="cmd_vel_watchdog" respawn="true" name="cmd_vel_watchdog" output="screen">
-
+    <arg name="velocity_topic" default="cmd_vel"/>
+    <node pkg="cmd_vel_watchdog"
+          type="cmd_vel_watchdog"
+          respawn="true"
+          name="cmd_vel_watchdog"
+          output="screen">
+        <remap from="cmd_vel" to="$(arg velocity_topic)"/>
     </node>
-    <!--<remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>-->
-    <!--TODO add remapping-->
+    <param name="watchdog_patience" value="10.0" type="double"/>
 </launch>
-<!--TODO parametrise node-->
diff --git a/cmd_vel_watchdog/src/cmd_vel_watchdog.cpp b/cmd_vel_watchdog/src/cmd_vel_watchdog.cpp
@@ -5,22 +5,25 @@
 #include <ros/ros.h>
 #include <geometry_msgs/Twist.h>
 
+namespace cmd_vel_watchdog{
+
 ros::Publisher *pub_ptr;
 ros::Timer *global_timer;
 geometry_msgs::Twist *stop_msg_ptr;
 
-static const ros::Duration *DEFAULT_WATCHDOG_TIMEOUT;
-void watchdogTimerCallback(const ros::TimerEvent &timerEvent);
+static const double DEFAULT_WATCHDOG_TIMEOUT (3.0);
+ros::Duration *watchdog_duration;
+double watchdog_timeout_param (3.0);
 
 void watchdogTimerCallback(const ros::TimerEvent &timerEvent){
-    ROS_WARN_STREAM("velocity topic callback activated!");
+    ROS_WARN_STREAM("velocity topic watchdog activated!");
     pub_ptr->publish(*stop_msg_ptr);
-    ROS_WARN_STREAM("STOP MSG PUBLISHED!!!");
+    ROS_WARN_STREAM("STOP MSG PUBLISHED to [" << pub_ptr->getTopic() << "]");
     global_timer->stop();
 }
 
 void initGlobals(ros::NodeHandle &nh){
-    DEFAULT_WATCHDOG_TIMEOUT = new ros::Duration(3.0);
+    watchdog_duration = new ros::Duration(watchdog_timeout_param);
 
     stop_msg_ptr = new geometry_msgs::Twist();
     stop_msg_ptr->linear.x = 0;
@@ -30,13 +33,12 @@ void initGlobals(ros::NodeHandle &nh){
     stop_msg_ptr->angular.y = 0;
     stop_msg_ptr->angular.z = 0;
 
-    global_timer = new ros::Timer(nh.createTimer(*DEFAULT_WATCHDOG_TIMEOUT, watchdogTimerCallback));
+    global_timer = new ros::Timer(nh.createTimer(*watchdog_duration, watchdogTimerCallback));
     global_timer->start();
 }
 
 bool isZeroSpeed(const geometry_msgs::Twist &msgIn){
-    return
-    msgIn.angular.z != 0 &&
+    return msgIn.angular.z != 0 &&
     msgIn.linear.x != 0 &&
     msgIn.angular.x != 0 &&
     msgIn.angular.y != 0 &&
@@ -54,42 +56,48 @@ void commandVelocityReceived(const geometry_msgs::Twist &msgIn){
                                     msgIn.angular.x <<" " << msgIn.angular.y <<" " << msgIn.angular.z
     );
     if ( ! isZeroSpeed(msgIn) ) {
-        global_timer->setPeriod(*DEFAULT_WATCHDOG_TIMEOUT, true);
+        global_timer->setPeriod(*watchdog_duration, true);
         global_timer->start();
         ROS_DEBUG_STREAM("timer reset");
     }
 }
 
 
+} // namespace cmd_vel_watchdog
 int main(int argc, char** argv){
     ros::init(argc, argv, "cmd_vel_watchdog");
     ros::NodeHandle nh;
 
-    initGlobals(nh);
+    if (!nh.getParam("watchdog_patience", cmd_vel_watchdog::watchdog_timeout_param)){
+        ROS_INFO_STREAM(
+                "watchdog_patience param is not found, setting to default = " <<
+                                                                              cmd_vel_watchdog::DEFAULT_WATCHDOG_TIMEOUT << " sec");
+        cmd_vel_watchdog::watchdog_timeout_param = cmd_vel_watchdog::DEFAULT_WATCHDOG_TIMEOUT;
+    }
+    ROS_INFO_STREAM("watchdog time set to " << cmd_vel_watchdog::watchdog_timeout_param << " [sec]");
+
+    cmd_vel_watchdog::initGlobals(nh);
+
+
 
     const std::string &default_velocity_topic = "cmd_vel";
     const std::string &this_node_name = ros::this_node::getName();
     ROS_INFO_STREAM(this_node_name << " created");
 
-    pub_ptr = new ros::Publisher(nh.advertise<geometry_msgs::Twist>(default_velocity_topic, 1));
+    cmd_vel_watchdog::pub_ptr = new ros::Publisher(nh.advertise<geometry_msgs::Twist>(default_velocity_topic, 1));
 
-    ROS_INFO_STREAM(this_node_name <<" advertised publisher to " <<pub_ptr->getTopic());
+    ROS_INFO_STREAM(this_node_name <<" advertised publisher to " << cmd_vel_watchdog::pub_ptr->getTopic());
 
-    ros::Subscriber sub  = nh.subscribe(default_velocity_topic, 1, &commandVelocityReceived);
+    ros::Subscriber sub  = nh.subscribe(default_velocity_topic, 1, &cmd_vel_watchdog::commandVelocityReceived);
     ROS_INFO_STREAM(this_node_name <<" subscribed to topic " << sub.getTopic());
 
     while(ros::ok()){
-//        for the case of problems with reconnects
-// TODO check if it works
-//        if (sub.getNumPublishers() == 0){
-//            sub  = nh.subscribe(default_velocity_topic, 1, &commandVelocityReceived);
-//        }
-//        ros::spinOnce();
-ros::spin();
+        ros::spin();
     }
 
     ROS_WARN_STREAM(this_node_name <<" SHUT_DOWN ");
-    delete pub_ptr;
-    delete global_timer;
-    delete stop_msg_ptr;
+    delete cmd_vel_watchdog::pub_ptr;
+    delete cmd_vel_watchdog::global_timer;
+    delete cmd_vel_watchdog::stop_msg_ptr;
+    delete cmd_vel_watchdog::watchdog_duration;
 }
diff --git a/explore_stack/launch/combined_bringup.launch b/explore_stack/launch/combined_bringup.launch
@@ -11,7 +11,8 @@
         <arg name="model" value="$(arg model)"/>
     </include>
 
-
+    <!--<include file="$(find cmd_vel_watchdog)/launch/cmd_vel_watchdog.launch">-->
+    <!--</include>-->
 
 
     <!--GAZEBO-->
@@ -118,5 +119,7 @@
         <param name="transform_tolerance" value="0.3"/>
         <param name="min_frontier_size" value="0.75"/>
     </node>
+
+
 
 </launch>