Change wam_node_test to hide interface thread.

diff --git a/.gitignore b/.gitignore
index c1edc29..14b6454 100644 (file)
--- a/.gitignore
+++ b/.gitignore
@@ -49,3 +49,6 @@ qtcreator-*
 
 # Catkin custom files
 CATKIN_IGNORE
+
+# Joe
+DEADJOE

diff --git a/wam_node_test/src/wam_node_test.cpp b/wam_node_test/src/wam_node_test.cpp
index 4ddc2c7..37d9aea 100644 (file)
--- a/wam_node_test/src/wam_node_test.cpp
+++ b/wam_node_test/src/wam_node_test.cpp
@@ -30,61 +30,73 @@
 
 #define sqr(x) ((x)*(x))
 
-class WamNodeTest
+class WamNodeOp
 {
        public:
-       WamNodeTest(const ros::NodeHandle &node);
-       ~WamNodeTest(void);
+       WamNodeOp(const ros::NodeHandle &node,double updateRate,double tolerance,double jointRateLimit);
+       ~WamNodeOp(void);
        void wamMove(const std::vector<float> &jpCmd);
        void wamWait(const std::vector<float> &jpCmd) const;
        void fingerMove(double f1,double f2,double f3);
        void fingerMove(double finger);
        void fingerWait(double f1,double f2,double f3) const;
        void fingerWait(double finger) const;
-       void spreadMove(double spread)
-       ;
+       void spreadMove(double spread);
        void spreadWait(double spread) const;
-       void update(void) const;
-       void test(void);
 
        private:
        
        void jointStatesCB(const sensor_msgs::JointState &jointStates);
+       void update(void) const;
        
        ros::NodeHandle node_;
        ros::Publisher jointPosPub_;
        ros::Subscriber jointStatesSub_;
-       
        ros::ServiceClient fingerPosCli_;
        ros::ServiceClient spreadPosCli_;
-       
+
        std::vector< std::atomic<float> > jpCmd_;
        std::vector< std::atomic<float> > wamJp_;
        std::vector< std::atomic<float> > bhandJp_;
 
+       std::thread update_;
+       bool runUpdate_;
+       
+       double updateRate_;
+       double tolerance_;
+       double jointRateLimit_;
 };
 
-WamNodeTest::WamNodeTest(const ros::NodeHandle &node):jpCmd_(7), wamJp_(7), bhandJp_(4)
+WamNodeOp::WamNodeOp(const ros::NodeHandle &node,double updateRate,double tolerance,double jointRateLimit):
+       jpCmd_(7), wamJp_(7), bhandJp_(4), runUpdate_(true)
 {
        node_=node;
        jointPosPub_=node_.advertise<wam_msgs::RTJointPos>("wam/jnt_pos_cmd",10);
        fingerPosCli_=node_.serviceClient<wam_srvs::BHandFingerPos>("bhand/finger_pos");
        spreadPosCli_=node_.serviceClient<wam_srvs::BHandSpreadPos>("bhand/spread_pos");
-       jointStatesSub_=node_.subscribe("/joint_states",10,&WamNodeTest::jointStatesCB,this);
+       jointStatesSub_=node_.subscribe("/joint_states",10,&WamNodeOp::jointStatesCB,this);
+       
+       tolerance_=tolerance;
+       updateRate_=updateRate;
+       jointRateLimit_=jointRateLimit;
+       
+       update_=std::thread(&WamNodeOp::update,this);
 }
 
-WamNodeTest::~WamNodeTest(void)
+WamNodeOp::~WamNodeOp(void)
 {
+       runUpdate_=false;
+       update_.join();
        jointPosPub_.shutdown();
        jointStatesSub_.shutdown();
 }
 
-void WamNodeTest::wamMove(const std::vector<float> &jpCmd)
+void WamNodeOp::wamMove(const std::vector<float> &jpCmd)
 {
        for(int i=0;i < jpCmd_.size() && i < jpCmd.size();i++) jpCmd_[i]=jpCmd[i];
 }
 
-void WamNodeTest::wamWait(const std::vector<float> &jpCmd) const
+void WamNodeOp::wamWait(const std::vector<float> &jpCmd) const
 {
        float error;
        do
@@ -93,11 +105,12 @@ void WamNodeTest::wamWait(const std::vector<float> &jpCmd) const
                error=0.0;      
                for(int i=0;i < wamJp_.size() && i < jpCmd.size();i++)
                        error+=sqr(wamJp_[i]-jpCmd[i]);
+               error=sqrt(error);
        }
-       while(error > 0.07);
+       while(error > 7*tolerance_);
 }
 
-void WamNodeTest::fingerMove(double f1,double f2,double f3)
+void WamNodeOp::fingerMove(double f1,double f2,double f3)
 {
        wam_srvs::BHandFingerPos fingerPosSrv;
        
@@ -108,29 +121,29 @@ void WamNodeTest::fingerMove(double f1,double f2,double f3)
        else ROS_ERROR_STREAM("Error in finger move.");
 }
 
-void WamNodeTest::fingerWait(double f1,double f2,double f3) const
+void WamNodeOp::fingerWait(double f1,double f2,double f3) const
 {
        float error;
        do
        {
                ros::Duration(0.01).sleep();
-               error=sqr(f1-bhandJp_[0])+sqr(f2-bhandJp_[1])+sqr(f3-bhandJp_[2]);
+               error=sqrt(sqr(f1-bhandJp_[0])+sqr(f2-bhandJp_[1])+sqr(f3-bhandJp_[2]));
        }
-       while(error > 0.03);
+       while(error > 3*tolerance_);
 }
 
-void WamNodeTest::fingerWait(double finger) const
+void WamNodeOp::fingerWait(double finger) const
 {
        fingerWait(finger,finger,finger);
 }
 
 
-void WamNodeTest::fingerMove(double finger)
+void WamNodeOp::fingerMove(double finger)
 {
        fingerMove(finger,finger,finger);
 }
 
-void WamNodeTest::spreadMove(double spread)
+void WamNodeOp::spreadMove(double spread)
 {
        wam_srvs::BHandSpreadPos spreadPosSrv;
        
@@ -139,27 +152,34 @@ void WamNodeTest::spreadMove(double spread)
        else ROS_ERROR_STREAM("Error in spread move.");
 }
 
-void WamNodeTest::spreadWait(double spread) const
+void WamNodeOp::spreadWait(double spread) const
 {
        float error;
        do
        {
                ros::Duration(0.01).sleep();
-               error=sqr(spread-bhandJp_[3]);
+               error=fabs(spread-bhandJp_[3]);
        }
-       while(error > 0.01);
+       while(error > tolerance_);
 }
 
-void WamNodeTest::update(void) const
+void WamNodeOp::update(void) const
 {
-       wam_msgs::RTJointPos jointPosMsg;
+       ros::Rate loop(updateRate_);
+       while(ros::ok() && runUpdate_)
+       {
+               wam_msgs::RTJointPos jointPosMsg;
+
+               for(int i=0;i < jpCmd_.size();i++) jointPosMsg.joints.push_back(jpCmd_[i]);
+               jointPosMsg.rate_limits.resize(jpCmd_.size(),jointRateLimit_);
+               jointPosPub_.publish(jointPosMsg);
 
-       for(int i=0;i < jpCmd_.size();i++) jointPosMsg.joints.push_back(jpCmd_[i]);
-       jointPosMsg.rate_limits.resize(jpCmd_.size(),0.5);
-       jointPosPub_.publish(jointPosMsg);
+               ros::spinOnce();
+               loop.sleep();
+       }
 }
 
-void WamNodeTest::jointStatesCB(const sensor_msgs::JointState &jointStates)
+void WamNodeOp::jointStatesCB(const sensor_msgs::JointState &jointStates)
 {
        std::vector<std::string> jointNames;
        jointNames.push_back("wam_joint_1");
@@ -187,65 +207,50 @@ void WamNodeTest::jointStatesCB(const sensor_msgs::JointState &jointStates)
        }
 }
 
-
-void WamNodeTest::test(void)
+int main(int argc,char* argv[])
 {
+       ros::init(argc,argv,"wam_node_test");
+       ros::NodeHandle node;
+       
+       WamNodeOp wamCmd(node,50.0,0.01,0.5);
+
        std::vector<float> jpCmd(7,0);
        jpCmd[3]=M_PI_2;
-       wamMove(jpCmd);
-       wamWait(jpCmd);
+       wamCmd.wamMove(jpCmd);
+       wamCmd.wamWait(jpCmd);
 
-       spreadMove(M_PI);
-       spreadWait(M_PI);
+       wamCmd.spreadMove(M_PI);
+       wamCmd.spreadWait(M_PI);
        
-       fingerMove(M_PI_2);
-       fingerWait(M_PI_2);
+       wamCmd.fingerMove(M_PI_2);
+       wamCmd.fingerWait(M_PI_2);
        
-       fingerMove(2.44);
-       fingerWait(2.44);
+       wamCmd.fingerMove(2.44);
+       wamCmd.fingerWait(2.44);
        
-       fingerMove(0.0);
-       fingerWait(0.0);
+       wamCmd.fingerMove(0.0);
+       wamCmd.fingerWait(0.0);
        
-       spreadMove(0);
-       spreadWait(0);
+       wamCmd.spreadMove(0);
+       wamCmd.spreadWait(0);
        
-       fingerMove(2.44,2.44,0.0);
-       fingerWait(2.44,2.44,0.0);
+       wamCmd.fingerMove(2.44,2.44,0.0);
+       wamCmd.fingerWait(2.44,2.44,0.0);
        
-       fingerMove(2.44);
-       fingerWait(2.44);
+       wamCmd.fingerMove(2.44);
+       wamCmd.fingerWait(2.44);
        
-       fingerMove(0.0);
-       fingerWait(0.0);
+       wamCmd.fingerMove(0.0);
+       wamCmd.fingerWait(0.0);
        
-       spreadMove(0.0);
-       spreadWait(0.0);
+       wamCmd.spreadMove(0.0);
+       wamCmd.spreadWait(0.0);
 
        std::fill(jpCmd.begin(),jpCmd.end(),0.0);
        jpCmd[1]=-2.0;
        jpCmd[3]=3.1;
-       wamMove(jpCmd); 
-       wamWait(jpCmd);
-       
-       exit(0);
-}
-
-int main(int argc,char* argv[])
-{
-       ros::init(argc,argv,"wam_node_test");
-       ros::NodeHandle node;
+       wamCmd.wamMove(jpCmd);  
+       wamCmd.wamWait(jpCmd);
        
-       WamNodeTest wamNodeTest(node);
-       std::thread test(&WamNodeTest::test,&wamNodeTest);
-
-       ros::Rate loop(50);
-       while(ros::ok())
-       {
-               wamNodeTest.update();
-               ros::spinOnce();
-               loop.sleep();
-       }
-
        return 0;
 }