From: Walter Fetter Lages Date: Mon, 10 Dec 2018 22:20:06 +0000 (-0200) Subject: Change wam_node_test to hide interface thread. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=91b74223779715b45320399eb5b8959f2c7a84d0;p=ufrgs_wam.git Change wam_node_test to hide interface thread. --- diff --git a/.gitignore b/.gitignore index c1edc29..14b6454 100644 --- 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 --- 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 &jpCmd); void wamWait(const std::vector &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 > jpCmd_; std::vector< std::atomic > wamJp_; std::vector< std::atomic > 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/jnt_pos_cmd",10); fingerPosCli_=node_.serviceClient("bhand/finger_pos"); spreadPosCli_=node_.serviceClient("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 &jpCmd) +void WamNodeOp::wamMove(const std::vector &jpCmd) { for(int i=0;i < jpCmd_.size() && i < jpCmd.size();i++) jpCmd_[i]=jpCmd[i]; } -void WamNodeTest::wamWait(const std::vector &jpCmd) const +void WamNodeOp::wamWait(const std::vector &jpCmd) const { float error; do @@ -93,11 +105,12 @@ void WamNodeTest::wamWait(const std::vector &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 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 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; }