From 579fb24e36bd60096f9b4f7e71c5b81ae989eada Mon Sep 17 00:00:00 2001
From: Adnan <adnan.munawar@live.com>
Date: Fri, 27 Oct 2017 16:33:35 -0400
Subject: [PATCH] Setting the timer_cb frequency explicitly during creation
 rather using rate::sleep()

---
 dvrk_arm/include/dvrk_arm/Bridge.h | 3 ++-
 dvrk_arm/src/Bridge.cpp            | 6 ++----
 2 files changed, 4 insertions(+), 5 deletions(-)

diff --git a/dvrk_arm/include/dvrk_arm/Bridge.h b/dvrk_arm/include/dvrk_arm/Bridge.h
index d27689e..e45d086 100644
--- a/dvrk_arm/include/dvrk_arm/Bridge.h
+++ b/dvrk_arm/include/dvrk_arm/Bridge.h
@@ -21,7 +21,7 @@ class DVRK_Bridge: public States, public DVRK_FootPedals{
     friend class DVRK_FootPedals;
     friend class DVRK_Console;
 
-    DVRK_Bridge(const std::string &arm_name);
+    DVRK_Bridge(const std::string &arm_name, int bridge_frequnce = 1000);
     ~DVRK_Bridge();
 
     template <class T, class U>
@@ -61,6 +61,7 @@ class DVRK_Bridge: public States, public DVRK_FootPedals{
     ros::Timer timer;
     AspinPtr aspin;
     RatePtr rate;
+    int _freq;
 
     double scale;
     bool _is_cnvFcn_set;
diff --git a/dvrk_arm/src/Bridge.cpp b/dvrk_arm/src/Bridge.cpp
index a1fab47..bb2da44 100644
--- a/dvrk_arm/src/Bridge.cpp
+++ b/dvrk_arm/src/Bridge.cpp
@@ -1,6 +1,6 @@
 #include "dvrk_arm/Bridge.h"
 
-DVRK_Bridge::DVRK_Bridge(const std::string &arm_name){
+DVRK_Bridge::DVRK_Bridge(const std::string &arm_name, int bridge_frequency): _freq(bridge_frequency){
     valid_arms.push_back("MTML");
     valid_arms.push_back("MTMR");
     valid_arms.push_back("PSM1");
@@ -30,13 +30,12 @@ void DVRK_Bridge::init(){
     char** argv;
     ros::M_string s;
     ros::init(s, arm_name + "_interface_node");
-
     n.reset(new ros::NodeHandle);
     nTimer.reset(new ros::NodeHandle);
     n->setCallbackQueue(&cb_queue);
     nTimer->setCallbackQueue(&cb_queue_timer);
     rate.reset(new ros::Rate(1000));
-    timer = nTimer->createTimer(ros::Duration(), &DVRK_Bridge::timer_cb, this);
+    timer = nTimer->createTimer(ros::Duration(1/(double)_freq), &DVRK_Bridge::timer_cb, this);
     aspin.reset(new ros::AsyncSpinner(0, &cb_queue_timer));
 
     pose_sub = n->subscribe("/dvrk/" + arm_name + "/position_cartesian_current", 10, &DVRK_Bridge::pose_sub_cb, this);
@@ -86,7 +85,6 @@ void DVRK_Bridge::state_sub_cb(const std_msgs::StringConstPtr &msg){
 }
 
 void DVRK_Bridge::timer_cb(const ros::TimerEvent& event){
-    _rate_sleep();
     cb_queue.callAvailable();
     if(_start_pubs == true){
         switch (activeState) {