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 (file)
--- 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<DOF>::moveTo(const jp_type& destination, bool blocking, double v
 template<size_t DOF>
 inline void Wam<DOF>::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<size_t DOF>
 inline void Wam<DOF>::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<size_t DOF>
 inline void Wam<DOF>::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<DOF>::moveTo(const T& currentPos, /*const typename T::unitless_type& cu
 {
        std::atomic<bool> started;
        started = false;
-//     boost::promise<boost::thread*> threadPtrPromise;
-//     boost::shared_future<boost::thread*> threadPtrFuture(threadPtrPromise.get_future());
-//     boost::thread* threadPtr = new boost::thread(&Wam<DOF>::moveToThread<T>, 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<T,jp_type>::value)
-//     thread=std::thread(&Wam<DOF>::moveToThread,this,currentPos,destination,velocity,acceleration,&started);
-//     else 
        thread=std::thread(&Wam<DOF>::moveToThread<T>,this,currentPos,destination,velocity,acceleration,&started);
 
        // wait until move starts
@@ -326,25 +318,19 @@ template<size_t DOF>
 template<typename T>
 void Wam<DOF>::moveToThread(const T &currentPos,const T &destination,double velocity,double acceleration,std::atomic<bool> *started)
 {
-       if(!std::is_same<T,jp_type>::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<KDL::VelocityProfile_Trap> trajectory(DOF);
+       std::vector<KDL::VelocityProfile_Trap> 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<DOF>::moveToThread(const T &currentPos,const T &destination,double velo
        doneMoving=false;
        *started=true;
 
-       jp_type jpLimit=jp_type::Constant(2.0);
-       wam_node_sim::RateLimiter<jp_type> 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<T> 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<size_t DOF>
-template<typename T>
-*/
-//void Wam<DOF>::moveToThread(const T& currentPos, /*const typename T::unitless_type& currentVel,*/ const T& destination, double velocity, double acceleration, bool* started, boost::shared_future<boost::thread*> threadPtrFuture)
-/*
-void Wam<DOF>::moveToThread(const T& currentPos, const T& destination, double velocity, double acceleration, std::atomic<bool>* started)
-{
-
-       // Only remove this thread from mtThreadGroup on orderly exit. (Don't remove on exception.)
-//     bool removeThread = false;
-       
-//     try {
-//             std::vector<T, Eigen::aligned_allocator<T> > vec;
-//             vec.push_back(currentPos);
-//             vec.push_back(destination);
-
-               // TODO(dc): Use currentVel. Requires changes to math::spline<Eigen::Quaternion<T> > specialization.
-               //math::Spline<T> spline(vec, currentVel);
-               //math::TrapezoidalVelocityProfile profile(velocity, acceleration, currentVel.norm(), spline.changeInS());
-//             math::Spline<T> spline(vec);
-//             math::TrapezoidalVelocityProfile profile(velocity, acceleration, 0.0, spline.changeInS());
-
-//             Ramp time(NULL, 1.0);
-//             Callback<double, T> 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<size_t DOF>
 void Wam<DOF>::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 (file)
--- 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 <vector>
 #include <atomic>
 
-//#include <boost/thread.hpp>
 #include <Eigen/Core>
 #include <Eigen/Geometry>
-//#include <libconfig.h++>
 
 #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<typename T> T currentPosHelper(const T& currentPos);
-//     template<typename T> void moveToThread(const T& currentPos, const T& destination, double velocity, double acceleration, bool* started, boost::shared_future<boost::thread*> threadPtrFuture);
        template<typename T> void moveToThread(const T& currentPos, const T& destination, double velocity, double acceleration, std::atomic<bool>* started);
-//     void moveToThread(const jp_type &currentPos,const jp_type &destination,double velocity,double acceleration,std::atomic<bool> *started);
 
        std::atomic<bool> 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<DOF> 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);