Add inactivity timer: if no activity after timeout, zero twist

Add an inactivity timer. Some wireless joysticks constantly transmit
data (such as the Play Station Dualshock4). In such cases, it is
possible to determine that the joystick is out of range and stop
motion by the fact that activity slows down or ceases. This is useful
for teleop in the case that the robot leaves range and a twist gets
stuck commanding the robot to move without stopping. In this case, the
inactivity timeout would detect no more messages after the threshold
and send a zero twist (just as if the enable button were released).
The default is for the inactivity timeout to be disabled. It can be
enabled by setting the inactivity_timeout parameter.

src/teleop_twist_joy.cpp

@@ -57,6 +57,12 @@ struct TeleopTwistJoy::Impl
   std::map< std::string, std::map<std::string, double> > scale_angular_map;
 
   bool sent_disable_msg;
+
+  double inactivity_timeout;
+  void restart_inactivity_timer(void);
+  void stop_inactivity_timer(void);
+  ros::Timer timer;
+  void inactivityTimerCallback(const ros::TimerEvent& e);
 };
 
 /**
@@ -122,6 +128,15 @@ TeleopTwistJoy::TeleopTwistJoy(ros::NodeHandle* nh, ros::NodeHandle* nh_param)
   }
 
   pimpl_->sent_disable_msg = false;
+
+  // if the inactivity_timeout is <= 0, it is disabled
+  nh_param->param<double>("inactivity_timeout", pimpl_->inactivity_timeout, -1.0);
+  // if the inactivity_timout is enabled, create a one-shot timer that is stopped
+  if (pimpl_->inactivity_timeout > 0.0)
+  {
+    pimpl_->timer = nh->createTimer(ros::Duration(pimpl_->inactivity_timeout),
+        &TeleopTwistJoy::Impl::inactivityTimerCallback, pimpl_, true, false);
+  }
 }
 
 double getVal(const sensor_msgs::Joy::ConstPtr& joy_msg, const std::map<std::string, int>& axis_map,
@@ -152,6 +167,7 @@ void TeleopTwistJoy::Impl::sendCmdVelMsg(const sensor_msgs::Joy::ConstPtr& joy_m
 
   cmd_vel_pub.publish(cmd_vel_msg);
   sent_disable_msg = false;
+  restart_inactivity_timer();
 }
 
 void TeleopTwistJoy::Impl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg)
@@ -177,8 +193,38 @@ void TeleopTwistJoy::Impl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg
       geometry_msgs::Twist cmd_vel_msg;
       cmd_vel_pub.publish(cmd_vel_msg);
       sent_disable_msg = true;
+      stop_inactivity_timer();
     }
   }
 }
 
+void TeleopTwistJoy::Impl::restart_inactivity_timer(void)
+{
+  if (inactivity_timeout > 0.0)
+  {
+    timer.stop();
+    timer.setPeriod(ros::Duration(inactivity_timeout));
+    timer.start();
+  }
+}
+
+void TeleopTwistJoy::Impl::stop_inactivity_timer(void)
+{
+  if (inactivity_timeout > 0.0)
+  {
+    timer.stop();
+  }
+}
+
+void TeleopTwistJoy::Impl::inactivityTimerCallback(const ros::TimerEvent& e)
+{
+  if (!sent_disable_msg)
+  {
+    geometry_msgs::Twist cmd_vel_msg;
+    ROS_INFO_NAMED("TeleopTwistJoy", "Joystick timed out, stopping motion");
+    cmd_vel_pub.publish(cmd_vel_msg);
+    sent_disable_msg = true;
+  }
+}
+
 }  // namespace teleop_twist_joy