Move parts of ChainPublisher's implementation from orogen component to lib (#6)

* test moving functionality to library

* modify .orogen file to point to library for robot_frameTypes

* simplify component code for updated robot_frames library

* bug fix
fixed a pointer and deleting the pointer at cleanup
modify variable name
fix loop iteration

* clear ports and vector for every configuration

---------

Authored-by: Ravisankar Selvaraju <[email protected]>

Author: RavisankarSelvaraju

robot_frames.orogen

@@ -1,7 +1,7 @@
 name "robot_frames"
 using_library "robot_frames"
 
-import_types_from "robot_framesTypes.hpp"
+import_types_from "robot_frames/robot_framesTypes.hpp"
 
 import_types_from "base"
 #import_types_from "transformer"

robot_framesTypes.hpp

@@ -18,60 +18,6 @@ ChainPublisher::~ChainPublisher()
 {
 }
 
-void ChainPublisher::clear_and_resize_vectors()
-{
-    chains_.clear();
-    chains_.resize(n_defined_chains_);
-
-    chain_names_.clear();
-    chain_names_.resize(n_defined_chains_);
-
-    for(size_t i=0; i<pos_solvers_.size(); i++){
-        delete pos_solvers_[i];
-    }
-    pos_solvers_.clear();
-    pos_solvers_.resize(n_defined_chains_);
-
-    kdl_frames_.clear();
-    kdl_frames_.resize(n_defined_chains_);
-
-    bt_frames_.clear();
-    bt_frames_.resize(n_defined_chains_);
-
-    joint_arrays_.clear();
-    joint_arrays_.resize(n_defined_chains_);
-
-    involved_active_joints_.clear();
-    involved_active_joints_.resize(n_defined_chains_);
-
-    for(size_t i=0; i<out_ports_.size(); i++){
-        ports()->removePort(out_ports_[i]->getName());
-        delete out_ports_[i];
-    }
-    out_ports_.clear();
-    out_ports_.resize(n_defined_chains_);
-}
-
-bool ChainPublisher::unpack_joints(const base::samples::Joints& joint_state,
-                                   const std::vector<std::string>& involved_joints,
-                                   KDL::JntArray& joint_array)
-{
-    assert(involved_joints.size() == joint_array.rows());
-
-    bool ok = true;
-    for(size_t i = 0; i<involved_joints.size(); i++){
-        try{
-            base::JointState js = joint_state.getElementByName(involved_joints[i]);
-            joint_array(i) = js.position;
-        }
-        catch(base::samples::Joints::InvalidName ex){
-            LOG_ERROR("Could not find joint %s in joint state vector.", involved_joints[i].c_str());
-            ok = false;
-        }
-    }
-
-    return ok;
-}
 
 /// The following lines are template definitions for the various state machine
 // hooks defined by Orocos::RTT. See ChainPublisher.hpp for more detailed
@@ -81,99 +27,22 @@ bool ChainPublisher::configureHook()
 {
     if (! ChainPublisherBase::configureHook())
         return false;
-
+    // clear the output_ports_ vector before configuration
+    output_ports_.clear();
+    
     std::vector<robot_frames::Chain> chain_definitions = _chains.get();
-    n_defined_chains_ = chain_definitions.size();
-    if(!n_defined_chains_){
-        LOG_WARN_S << "No chains are defined! This component will do nothing..." << std::endl;
-    }
-
     std::string urdf_file_path = _urdf_file.get();
 
-    bool st;
-    st = kdl_parser::treeFromFile(urdf_file_path, tree_);
-    if(!st){
-        LOG_ERROR_S << "Error parsing urdf file: " << urdf_file_path << std::endl;
-        return false;
-    }
-
-    //Resize stuff
-    clear_and_resize_vectors();
-
-    //Receive Chains from urdf
-    for(size_t i=0; i<n_defined_chains_; i++){
-        std::string root = chain_definitions[i].root_link;
-
-        //By entering '__base__' as root or tip link, one could refer to the actual base-link of the robot model
-        if(root == "__base__"){
-            root = tree_.getRootSegment()->first;
-        }
-        std::string tip = chain_definitions[i].tip_link;
-        if(tip == "__base__"){
-            tip = tree_.getRootSegment()->first;
-        }
-
-        std::string name = chain_definitions[i].name;
-
-        st = tree_.getChain(root, tip, chains_[i]);
-        if(!st){
-            LOG_ERROR("Error extracting chain '%s' with root '%s' and tip '%s'. The urdf_file is: %s",
-                      name.c_str(), root.c_str(), tip.c_str(), urdf_file_path.c_str());
-            return false;
-        }
-        chain_names_[i] = name;
-    }
-
-    //Determine invloved joits, prepare frames storage and solvers
-    for(size_t i=0; i<n_defined_chains_; i++){
-        KDL::Chain chain = chains_[i];
-        //Determine involved joints for each chain
-        for(uint s=0; s<chain.segments.size(); s++){
-            KDL::Segment segment = chain.segments[s];
-            std::string jname = segment.getJoint().getName();
-
-            if(segment.getJoint().getType() == KDL::Joint::None){
-                LOG_DEBUG("Skipping joint %s of chain %s. Joint is fixed.",
-                          jname.c_str(),
-                          chain_names_[i].c_str());
-                continue;
-            }
-
-            involved_active_joints_[i].push_back(jname);
-        }
-        //Resize joint arrays
-        joint_arrays_[i].resize(involved_active_joints_[i].size());
-        joint_arrays_[i].data.setZero();
-
-        //Prepare frames storage
-        base::samples::RigidBodyState rbs;
-        if(chain_definitions[i].tip_link_renamed == ""){
-            rbs.sourceFrame = chain_definitions[i].tip_link;
-        }
-        else{
-            rbs.sourceFrame = chain_definitions[i].tip_link_renamed;
-        }
-        if(chain_definitions[i].root_link_renamed == ""){
-            rbs.targetFrame = chain_definitions[i].root_link;
-        }
-        else{
-            rbs.targetFrame = chain_definitions[i].root_link_renamed;
-        }
-        rbs.invalidate();
-        bt_frames_[i] = rbs;
-
-        //Prepare solver
-        pos_solvers_[i] = new KDL::ChainFkSolverPos_recursive(chains_[i]);
-    }
+    // assign the created object to the chain_transformer pointer to access this object in update_hook 
+    chain_transformer = new ChainTransformationCalculator(chain_definitions, urdf_file_path);
 
     //Create dynamic output per chain
-    for(int i=0; i<n_defined_chains_; i++){
-        std::string port_name = chain_names_[i];
+    for(const std::string& port_name :  chain_transformer->get_chain_names()){
         LOG_INFO("Adding port %s to component", port_name.c_str());
         RTT::OutputPort<base::samples::RigidBodyState>* output_port =
                 new RTT::OutputPort<base::samples::RigidBodyState>(port_name);
         ports()->addPort(port_name, *output_port);
-        out_ports_[i] = output_port;
+        output_ports_.push_back(output_port);
     }
 
     return true;
@@ -188,28 +57,14 @@ void ChainPublisher::updateHook()
 {
     ChainPublisherBase::updateHook();
 
-    while (_input.read(joint_state_, false) == RTT::NewData){
-        int st;
-        //Go thourgh chains
-        for(size_t i=0; i<n_defined_chains_; i++){
-            KDL::ChainFkSolverPos_recursive* solver = pos_solvers_[i];
-
-            //Extract joint array for chain
-            if(!unpack_joints(joint_state_, involved_active_joints_[i], joint_arrays_[i]))
-                throw std::runtime_error("Unable to correctly unpack joint values on kinematic chain");
-
-            //Calculate
-            st = solver->JntToCart(joint_arrays_[i], kdl_frames_[i]);
-            if(st < 0){
-                LOG_ERROR_S << "Something went wrong solving forward kineamtics for chain " << chain_names_[i] << std::endl;
+    //clear bt_frames before each update
+    bt_frames_.clear();
+    while (_input.read(joint_state, false) == RTT::NewData){
+        chain_transformer->update_transforms(joint_state, bt_frames_);
+        for(int i=0; i< output_ports_.size(); i++){
+            output_ports_[i]->write(bt_frames_[i]);
             }
-
-            //Convert and write to port
-            convert(kdl_frames_[i], bt_frames_[i]);
-            bt_frames_[i].time = joint_state_.time;
-            out_ports_[i]->write(bt_frames_[i]);
         }
-    }
 }
 void ChainPublisher::errorHook()
 {
@@ -222,4 +77,10 @@ void ChainPublisher::stopHook()
 void ChainPublisher::cleanupHook()
 {
     ChainPublisherBase::cleanupHook();
+
+    // Remove all dynamically created ports
+    for(const std::string& port_name :  chain_transformer->get_chain_names())
+        ports()->removePort(port_name);
+        
+    delete chain_transformer;
 }

tasks/ChainPublisher.cpp

@@ -4,13 +4,12 @@
 #define ROBOT_FRAMES_CHAINPUBLISHER_TASK_HPP
 
 #include "robot_frames/ChainPublisherBase.hpp"
-#include "robot_framesTypes.hpp"
 #include <kdl_parser/kdl_parser.hpp>
 #include <kdl/chainfksolvervel_recursive.hpp>
 #include <kdl/chainfksolverpos_recursive.hpp>
 #include <kdl/jntarray.hpp>
 #include <base/samples/RigidBodyState.hpp>
-#include <base/Logging.hpp>
+#include <base-logging/Logging.hpp>
 
 #include <robot_frames/RobotFrames.hpp>
 
@@ -21,26 +20,14 @@ class ChainPublisher : public ChainPublisherBase
 {
     friend class ChainPublisherBase;
 protected:
-    std::vector<KDL::Chain> chains_;
-    std::vector<std::string> chain_names_;
-    std::vector<KDL::ChainFkSolverPos_recursive*> pos_solvers_;
-    std::vector<KDL::Frame> kdl_frames_;
-    std::vector<base::samples::RigidBodyState> bt_frames_;
-    std::vector<KDL::JntArray> joint_arrays_;
-    std::vector<std::vector<std::string> >involved_active_joints_;
-    base::samples::Joints joint_state_;
-    KDL::Tree tree_;
-    int n_defined_chains_;
-    std::vector<RTT::OutputPort<base::samples::RigidBodyState>*> out_ports_;
-
-    void clear_and_resize_vectors();
-    bool unpack_joints(const base::samples::Joints& joint_state,
-                       const std::vector<std::string>& involved_joints,
-                       KDL::JntArray& joint_array);
+     base::samples::Joints joint_state;
+     std::vector<base::samples::RigidBodyState> bt_frames_;
+     std::vector<RTT::OutputPort<base::samples::RigidBodyState>*> output_ports_;
+
+     ChainTransformationCalculator* chain_transformer;
 
 public:
     ChainPublisher(std::string const& name = "robot_frames::ChainPublisher");
-
     ChainPublisher(std::string const& name, RTT::ExecutionEngine* engine);
 
     ~ChainPublisher();