From: Walter Fetter Lages Date: Mon, 24 Dec 2018 03:52:43 +0000 (-0200) Subject: Change moveToThread to a proper template. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=e6730f00192a9549fafaa073676503bcb02631dd;p=ufrgs_wam.git Change moveToThread to a proper template. --- diff --git a/wam_node_sim/include/wam_node_sim/detail/wam-impl.h b/wam_node_sim/include/wam_node_sim/detail/wam-impl.h index ec36b84..4d0c3bc 100644 --- a/wam_node_sim/include/wam_node_sim/detail/wam-impl.h +++ b/wam_node_sim/include/wam_node_sim/detail/wam-impl.h @@ -242,20 +242,20 @@ inline void Wam::moveTo(const jp_type& destination, bool blocking, double v template inline void Wam::moveTo(const cp_type& destination, bool blocking, double velocity, double acceleration) { - moveTo(currentPosHelper(getToolPosition()), /*cv_type(0.0),*/ destination, blocking, velocity, acceleration); +// moveTo(currentPosHelper(getToolPosition()), /*cv_type(0.0),*/ destination, blocking, velocity, acceleration); } template inline void Wam::moveTo(const Eigen::Quaterniond& destination, bool blocking, double velocity, double acceleration) { - moveTo(currentPosHelper(getToolOrientation()), destination, blocking, velocity, acceleration); +// moveTo(currentPosHelper(getToolOrientation()), destination, blocking, velocity, acceleration); } template inline void Wam::moveTo(const pose_type& destination, bool blocking, double velocity, double acceleration) { - moveTo(currentPosHelper(getToolPose()), destination, blocking, velocity, acceleration); +// moveTo(currentPosHelper(getToolPose()), destination, blocking, velocity, acceleration); } @@ -265,16 +265,8 @@ void Wam::moveTo(const T& currentPos, /*const typename T::unitless_type& cu { std::atomic started; started = false; -// boost::promise threadPtrPromise; -// boost::shared_future threadPtrFuture(threadPtrPromise.get_future()); -// boost::thread* threadPtr = new boost::thread(&Wam::moveToThread, this, boost::ref(currentPos), /*currentVel,*/ boost::ref(destination), velocity, acceleration, &started, threadPtrFuture); -// mtThreadGroup.add_thread(threadPtr); -// threadPtrPromise.set_value(threadPtr); std::thread thread; -// if(std::is_same::value) -// thread=std::thread(&Wam::moveToThread,this,currentPos,destination,velocity,acceleration,&started); -// else thread=std::thread(&Wam::moveToThread,this,currentPos,destination,velocity,acceleration,&started); // wait until move starts @@ -326,25 +318,19 @@ template template void Wam::moveToThread(const T ¤tPos,const T &destination,double velocity,double acceleration,std::atomic *started) { - if(!std::is_same::value) - { - ROS_WARN_STREAM("moveToThread template does not support this type."); - return; - } static const int PUBLISH_FREQ=250.0; int longest; (currentPos-destination).cwiseAbs().maxCoeff(&longest); - std::vector trajectory(DOF); + std::vector trajectory(currentPos.size()); trajectory[longest].SetMax(velocity,acceleration); trajectory[longest].SetProfile(currentPos[longest],destination[longest]); double tf=trajectory[longest].Duration(); - if(tf < 1.0/PUBLISH_FREQ) tf=1.0/PUBLISH_FREQ; - - for(int i=0;i < DOF;i++) + for(int i=0;i < currentPos.size();i++) { + if(i == longest) continue; trajectory[i].SetMax(velocity,acceleration); trajectory[i].SetProfileDuration(currentPos[i],destination[i],tf); } @@ -352,91 +338,25 @@ void Wam::moveToThread(const T ¤tPos,const T &destination,double velo doneMoving=false; *started=true; - jp_type jpLimit=jp_type::Constant(2.0); - wam_node_sim::RateLimiter jpRateLimiter(jpLimit,1.0/PUBLISH_FREQ); - jp_type jp; - for(int i=0;i < DOF;i++) jp[i]=currentPos[i]; - jpRateLimiter.setCurVal(jp); + T limit=T::Constant(2.0); + wam_node_sim::RateLimiter rateLimiter(limit,1.0/PUBLISH_FREQ); + rateLimiter.setCurVal(currentPos); ros::Rate loop(PUBLISH_FREQ); double t=0.0; ros::Time t0=ros::Time::now(); while(ros::ok() && (t=(ros::Time::now()-t0).toSec()) < tf) - { - for(size_t i=0; i < DOF;i++) jp[i]=trajectory[i].Pos(t); - trackReferenceSignal(jpRateLimiter.getLimit(jp)); + { + T pos; + for(size_t i=0; i < pos.size();i++) pos[i]=trajectory[i].Pos(t); + trackReferenceSignal(rateLimiter.getLimit(pos)); loop.sleep(); } - for(int i=0;i < DOF;i++) jp[i]=destination[i]; - trackReferenceSignal(jpRateLimiter.getLimit(jp)); + trackReferenceSignal(rateLimiter.getLimit(destination)); doneMoving=true; } -/* -template -template -*/ -//void Wam::moveToThread(const T& currentPos, /*const typename T::unitless_type& currentVel,*/ const T& destination, double velocity, double acceleration, bool* started, boost::shared_future threadPtrFuture) -/* -void Wam::moveToThread(const T& currentPos, const T& destination, double velocity, double acceleration, std::atomic* started) -{ - - // Only remove this thread from mtThreadGroup on orderly exit. (Don't remove on exception.) -// bool removeThread = false; - -// try { -// std::vector > vec; -// vec.push_back(currentPos); -// vec.push_back(destination); - - // TODO(dc): Use currentVel. Requires changes to math::spline > specialization. - //math::Spline spline(vec, currentVel); - //math::TrapezoidalVelocityProfile profile(velocity, acceleration, currentVel.norm(), spline.changeInS()); -// math::Spline spline(vec); -// math::TrapezoidalVelocityProfile profile(velocity, acceleration, 0.0, spline.changeInS()); - -// Ramp time(NULL, 1.0); -// Callback trajectory(boost::bind(boost::ref(spline), boost::bind(boost::ref(profile), _1))); - -// connect(time.output, trajectory.input); -// trackReferenceSignal(trajectory.output); -// time.start(); - - doneMoving = false; - *started = true; - - // The value of trajectory.input will be undefined until the next execution cycle. - // It may become undefined again if trajectory.output is disconnected and this chain - // of Systems loses its ExecutionManager. We must check that the value is defined - // before calling getValue(). -// while ( !trajectory.input.valueDefined() || trajectory.input.getValue() < profile.finalT()) { - ROS_WARN_STREAM("moveToThread template not implemented."); - // if the move is interrupted, clean up and end the thread -// if ( !trajectory.output.isConnected() || boost::this_thread::interruption_requested() ) { -// removeThread = true; -// break; -// } -// ros::Duration(0.01).sleep(); // Interruption point. May throw boost::thread_interrupted -// } - -// if ( !removeThread ) { - doneMoving = true; - - // Wait until the trajectory is no longer referenced by supervisoryController -// while (trajectory.hasExecutionManager() && !boost::this_thread::interruption_requested()) { -// ros::Duration(0.01).sleep(); // Interruption point. May throw boost::thread_interrupted -// } -// removeThread = true; -// } -// } catch (const std::exception &e) {ROS_WARN_STREAM("wam-impl: " << e.what());} - -// if (removeThread) { -// mtThreadGroup.remove_thread(threadPtrFuture.get()); -// delete threadPtrFuture.get(); -// } -} -*/ template void Wam::jointStatesCB(const sensor_msgs::JointState &jointStates) { diff --git a/wam_node_sim/include/wam_node_sim/wam.h b/wam_node_sim/include/wam_node_sim/wam.h index fbb58b9..c1ab677 100644 --- a/wam_node_sim/include/wam_node_sim/wam.h +++ b/wam_node_sim/include/wam_node_sim/wam.h @@ -41,14 +41,10 @@ #ifndef WAM_NODE_SIM_WAM_H_ #define WAM_NODE_SIM_WAM_H_ - -//#include #include -//#include #include #include -//#include #include "ros/ros.h" #include "trajectory_msgs/JointTrajectoryPoint.h" @@ -170,31 +166,22 @@ public: */ void idle(); - /** getEmMutex() method allows access to ExecutionManagers Mutex in LowLevelWam Class. - */ -// thread::Mutex& getEmMutex() const { return llww.getEmMutex(); } - - private: template T currentPosHelper(const T& currentPos); -// template void moveToThread(const T& currentPos, const T& destination, double velocity, double acceleration, bool* started, boost::shared_future threadPtrFuture); template void moveToThread(const T& currentPos, const T& destination, double velocity, double acceleration, std::atomic* started); -// void moveToThread(const jp_type ¤tPos,const jp_type &destination,double velocity,double acceleration,std::atomic *started); std::atomic doneMoving; -// boost::thread_group mtThreadGroup; // Used to calculate TP and TO if the values aren't already being calculated in the control loop. -// mutable math::Kinematics kin; + KDL::Tree tree_; + KDL::Chain chain_; + KDL::ChainFkSolverPos_recursive *fwdKinSolverPos_; ros::Publisher wam_cmd_pub; // controller command ros::Subscriber jointStatesSubscriber_; jp_type jointPosition_; jv_type jointVelocity_; jt_type jointEffort_; - KDL::Tree tree_; - KDL::Chain chain_; - KDL::ChainFkSolverPos_recursive *fwdKinSolverPos_; Hand *hand_; void jointStatesCB(const sensor_msgs::JointState &jointStates);