Dynamically load the wanted HAL

[torydebra]

So far we have :

//ROSEE::EEHal::Ptr eeHalPtr = std::make_shared<ROSEE::DummyHal>(&nh);
//ROSEE::EEHal::Ptr eeHalPtr = std::make_shared<ROSEE::XBot2Hal>(&nh);
ROSEE::EEHal::Ptr eeHalPtr = std::make_shared<ROSEE::HeriIIXBotHal>(&nh);

How can we load dynamically the concrete class ? I found :

• Using dl https://stackoverflow.com/questions/496664/c-dynamic-shared-library-on-linux
• Using poco https://pocoproject.org/slides/120-SharedLibraries.pdf

Which is the better way? Are there any other better methods? @liesrock

[liesrock]

dl is the right way to go imho: anyhow you can take a look at xbot2_wip implementation to take inspiration from @alaurenzi.

https://github.com/ADVRHumanoids/xbot2_wip/blob/738466b9a5c55cfba5d85e7eb71931ede8ad5dab/src/utils/load_object.h

[torydebra]

Thanks.

If I understood well, with dl and dlsym we will work with a classic pointer? We can not return a shared_ptr?

[alaurenzi]

Since you load a plugin by invoking a factory method with C linkage, it's not recommended to pass/return C++ objects. Of course, on the C++ side, you immediately wrap the raw pointer inside a smart pointer

[torydebra]

I created a utils function:

template <typename RetType>
std::shared_ptr<RetType> loadObject(std::string lib_name, std::string function_name) {

    if (lib_name.empty()) {

        std::cerr << "[Utils::loadObject] ERROR: Please specify lib_name" << std::endl;
        return nullptr;
    } 

    void* lib_handle = dlopen(lib_name.c_str(), RTLD_LAZY);
    if (!lib_handle) {
        std::cerr << "[Utils::loadObject] ERROR in opening the library:" << dlerror() << std::endl;
        return nullptr;
    }

    RetType* (*function)();
    function = dlsym(lib_handle, function_name.c_str());
    if (dlerror() != NULL)  {
        std::cerr << "[Utils::loadObject] ERROR in returning the function:" << dlerror() << std::endl;
        return nullptr;
    }

    RetType* objectRaw = function();

    std::shared_ptr<RetType> objectPtr(objectRaw);

    dlclose(lib_handle);

    return objectPtr;
}

Now, the problem is how to put a pure virtual create object in the base class, such that the derived class are forced to implement it. I can't:

class EEHal  {
   ...
   virtual extern "C" EEHal* create_object() = 0;
   ...
}

Since C does not have method and classes.

[alaurenzi]

You use a factory free-function (usually auto-generated by a REGISTER-like macro). See here.

[torydebra]

You use a factory free-function (usually auto-generated by a REGISTER-like macro). See here.

Thanks!

I have also to implement a destroy_object ? I saw lot of examples online always putting one, but I do not see it in xbot2

[torydebra]

Ok, I think I have done. I put here stuff for reference.

The Utils function to use dl:

template <typename RetType, typename... Args>
std::unique_ptr<RetType> loadObject(std::string lib_name, 
                                    std::string function_name,
                                    Args... args) {

    if (lib_name.empty()) {

        std::cerr << "[Utils::loadObject] ERROR: Please specify lib_name" << std::endl;
        return nullptr;
    } 

    std::string lib_name_path = "lib" + lib_name +".so"; 

    void* lib_handle = dlopen(lib_name_path.c_str(), RTLD_LAZY);
    if (!lib_handle) {
        std::cerr << "[Utils::loadObject] ERROR in opening the library: " << dlerror() << std::endl;
        return nullptr;
    }

    RetType* (*function)(Args... args);
    function = reinterpret_cast<RetType* (*)(Args... args)>(dlsym(lib_handle, function_name.c_str()));
    if (dlerror() != NULL)  {
        std::cerr << "[Utils::loadObject] ERROR in returning the function: " << dlerror() << std::endl;
        return nullptr;
    }

    RetType* objectRaw = function(args...);

    std::unique_ptr<RetType> objectPtr(objectRaw);

    dlclose(lib_handle);

    return objectPtr;
}

The macro that must be used in each derived HAL:

#define HAL_CREATE_OBJECT(className) \
    extern "C" ::ROSEE::EEHal* create_object_##className ( ros::NodeHandle* nh) { \
        return new className(nh); \
    } \

The "main" which uses the pointer:

td::string hal_lib;
    if ( ! nh.getParam ( "/rosee/hal_library_name", hal_lib ) ) {
        ROS_ERROR_STREAM( "Ros parameter 'hal_library_name' not found" );
        return -1;
    }

    std::unique_ptr<ROSEE::EEHal> eeHalPtr = ROSEE::Utils::loadObject<ROSEE::EEHal, ros::NodeHandle*>
                                         (hal_lib, "create_object_"+hal_lib, &nh);

    if (eeHalPtr == nullptr) {
        ROS_ERROR_STREAM( "[EEHalExecutor ERROR] in loading the EEHal Object" );
        return -1;    
    }

    ROS_INFO_STREAM ( "[EEHalExecutor] Loaded "<<  hal_lib << " HAL"  );   

    //TODO take rate from param
    ros::Rate r(100);
    while(ros::ok()) {

        //TODO check order of these

        //receive info from robot, and fill _js_msg
        eeHalPtr->sense();

        //make the robot move following the refs in _mr_msg
        eeHalPtr->move();

        //send _js_msg to external (ie to ROSEE main node)
        eeHalPtr->publish_joint_state();

        ros::spinOnce();
        r.sleep();      
    }

EEHal base class header

 class EEHal  {

    public:

        typedef std::shared_ptr<EEHal> Ptr;
        typedef std::shared_ptr<const EEHal> ConstPtr;

        EEHal ( ros::NodeHandle* nh );
        virtual ~EEHal() {};

        virtual bool sense() = 0;

        virtual bool move() = 0;

        virtual bool publish_joint_state();

        //virtual bool setMotorPositionReference(std::string motor, double pos) = 0;
        //virtual bool getJointPosition(std::string joint, std::vector<double> &pos ) = 0;

    protected:

        ros::NodeHandle* _nh; 

        /**
         * The references that must be read in the move() to send to the real/simulated robot
         * TODO put private and create a get ? (no set)
         */
        sensor_msgs::JointState _mr_msg;
        ros::Subscriber _motor_reference_sub;

        /**
         * The states that must be filled in the sense(), reading info from real/simulated robot
         * TODO put private and create a set (no get) ?
         */
        sensor_msgs::JointState _js_msg;
        ros::Publisher _joint_state_pub;

    private:
        void motor_reference_clbk(const sensor_msgs::JointState::ConstPtr& msg);

    };

DummyHal derived class example

class DummyHal : public EEHal {

    public:

        typedef std::shared_ptr<DummyHal> Ptr;
        typedef std::shared_ptr<const DummyHal> ConstPtr;

        DummyHal( ros::NodeHandle* nh);
        virtual ~DummyHal() {};

        virtual bool sense() override;
        virtual bool move() override;

    private:
        /**
         * @brief this will publish to joint_state_publisher
         */
        ros::Publisher _hal_joint_state_pub;

        /**
         * @brief this will subscribe to joint_state_publisher
         */
        ros::Subscriber _hal_joint_state_sub;

        void hal_js_clbk(const sensor_msgs::JointState::ConstPtr& msg);

    };

HAL_CREATE_OBJECT(DummyHal)    

Please tell me if you notice something wrong/ugly :D

[alaurenzi]

You use a factory free-function (usually auto-generated by a REGISTER-like macro). See here.

Thanks!

I have also to implement a destroy_object ? I saw lot of examples online always putting one, but I do not see it in xbot2

Since your class correctly uses virtual destructors, it's not needed

[alaurenzi]

All good! Only thing, feel free to use pass-by-value / reference in the constructor, you just need to deference the pointer when calling it; also, you can probably omit the second template parameter when calling the factory

[torydebra]

also, you can probably omit the second template parameter when calling the factory

Right the argument is useless! I was thinking at the factory as the costructor :D Sorry I misunderstood. Yes I removed the nodehandle type when calling the loadObject

feel free to use pass-by-value / reference in the constructor, you just need to deference the pointer when calling it

Regarding this, I usually like to pass the pointer such that who uses the caller explicity writes "&var" and do not forget that he is passing the same object, and that, for example, its variable could be modified