[XBot How To] Do I have to use a ControlPlugin to use the device/driver hyerarchy?

Question

[XBot How To] Do I have to use a ControlPlugin to use the device/driver hyerarchy?

Closed this issue 4 years ago · 7 comments

I am following the code from device folder, where Device, DeviceClient, Driver and their containers are defined.
At the top, there is the rocket_plugin (which derives ControlPlugin) that does:
_rocket = _robot->getDeviceInstance<Hal::RocketBase>("rocket_0");
To have the _rocket and its set, get methods.

I would like to have a similar things, but I do not want the node which uses the _rocket to be a ControlPlugin: because I do not want to define the on_init, starting, run ... methods.

So, what I have done is to define in a similar way all the driver, client and containers classes:

class XBotEEBase : virtual public XBot::Hal::DeviceBase
{

public:

    XBOT2_DECLARE_SMART_PTR(XBotEEBase)

    virtual bool setMotorPositionReference (std::string motor_name, double position_ref) = 0;
    
    virtual bool getMotorPosition (std::string motor_name, double &position) = 0; 

};   

using XBotEEClientBase = XBot::Hal::DeviceClientTpl<ROSEEPacket::Rx, ROSEEPacket::Tx>;
    

class XBotEEClient : public XBotEEClientBase,
                     public virtual XBotEEBase
{

public:

    using XBotEEClientBase::XBotEEClientBase;
    
    bool setMotorPositionReference (std::string motor_name, double position_ref) override;
    
    bool getMotorPosition (std::string motor_name, double &position) override; 

};

/**
 * @brief The RocketClientContainer class implements optional API
 * methods referring to multiple rockets together.
 */
class XBotEEClientContainer : public XBot::Hal::DeviceContainer<XBotEEClient>
{

public:

    using XBot::Hal::DeviceContainer<XBotEEClient>::DeviceContainer;

};



class XBotGazeboDriver : public XBot::Hal::DeviceDriverTpl<ROSEEPacket::Rx, ROSEEPacket::Tx>
{

public:

    XBotGazeboDriver(XBot::Hal::DeviceInfo devinfo);

    /**
     * @brief move_impl allows to override the tx data from
     * the framework before it's actually sent to the device
     */
    bool move_impl() override;

    /**
     * @brief sense_impl allows to override the rx data from
     * the device before it's actually sent to the framework
     * @return if return false, rx is not actually sent to
     * the framework!
     */
    bool sense_impl() override;

private:

    //std::chrono::nanoseconds _timeout;
};

/**
 * @brief The RocketDev class implements the client side
 * for all rocket device instances. If the protocol in use is
 * xbot2's client-server protocol, this is pure boilerplate
 * code that can be autogenerated. Otherwise, the user must
 * write the code to connect to the remote device.
 */
class XBotGazeboDriverContainer : public XBot::Hal::DeviceContainer<XBotGazeboDriver>
{

public:

    XBotGazeboDriverContainer(std::vector<XBot::Hal::DeviceInfo> devinfo);

    bool sense_all() override;

    bool move_all() override;

private:

    bool _srv_alive;

    XBot::ClientManager::UniquePtr _cli;

};

Now I want to access to XBotEEClient instance. I can not do a thing like _rocket = _robot->getDeviceInstance<Hal::RocketBase>("rocket_0"); because I do not have the _robot, because the class is not a ControlPlugin.

So I randomly tried to create the "_rocket" instance (named _xbot_ee_client here) :

_xbot_ee_client = std::make_shared<ROSEE::XBotEEClient>(devinfo);

But there is some linking issue:

[ 95%] Built target UniversalFindActions
[ 97%] Built target rosee_gui_MainWindow
[ 98%] Linking CXX executable /home/tori/ROSEE/devel/lib/ros_end_effector/UniversalRosEndEffector
[100%] Built target rosee_gui_gui_main
/home/tori/ROSEE/devel/lib/libXBot2Hal.so: undefined reference to `ROSEE::XBotEEClient::XBotEEClient(XBot::Hal::DeviceInfo)'
collect2: error: ld returned 1 exit status
make[2]: *** [/home/tori/ROSEE/devel/lib/ros_end_effector/UniversalRosEndEffector] Error 1

So I do not know if:

This is solvable, and I am missing some #include or some in the cmake
This is unsolvable because I am using uncorrectly xbot2

Answer 1 · 2020-10-02T09:55:51.000Z

Instead, if I had to use the ControlPlugin, how can I costruct such type of object?
The costructor wants an Args argument. I think this is filled in some part of the xbotcore main node.

But I would like to "control" my ControlPlugin giving to it a command like the _thrust (I am always looking at the rocket example) using the methods. In the example _thrust is read by a file or the ROS service I think. So the only way to communicate with the ControlPlugin is through services? I can not have a ControlPlugin as a member of a wrapper class?

Answer 2 · 2020-10-05T14:46:46.000Z

Please explain your needs. I assume that the only user of the framework is the implementer of a control plugin. From a developer's perspective, within the HAL system, you directly manipulate the _rx and _tx structs, instead.

Answer 3 · 2020-11-03T13:24:15.000Z

Some updates on this.

Tecnically, what I want is to access to the Robot

In these examples, _robot member is available because everything is a ControlPlugin

In xbot1 examples, there was another way:

std::string path_to_config_file = XBot::Utils::getXBotConfig();
auto robot = XBot::RobotInterface::getRobot(path_to_config_file);

This permit to access to robot from a node that is not a ControlPlugin

I tried to do this with xbot2. In the conf file, I had to add:

RobotInterface:
  framework_name: "ROS"

otherwise there was a point where some class complied that the "framework" has not been defined.

But now when calling
auto robot = XBot::RobotInterface::getRobot(path_to_config_file), I have this error:

[info] Generated joint id map as:
 joint_map:
  1: joint1
  2: joint2
  3: joint3
  4: joint4
  5: joint5
[ok  ] RobotInterface so library found! 
[ok  ] MODEL INTERFACE so library found! 
[info] Floating base model: FALSE
[info] Joint name: joint1 RBDL ID: 0
[info] Joint name: joint2 RBDL ID: 1
[info] Joint name: joint3 RBDL ID: 2
[info] Joint name: joint4 RBDL ID: 3
[info] Joint name: joint5 RBDL ID: 4
[ok  ] ModelInterfaceRBDL initialized successfully!
[info] Model ordered chains: 
arm

[info] Constructing ROS implementation of RobotInterface.. 
terminate called after throwing an instance of 'std::runtime_error'
  what():  Unable to connect to XBotCore
Annullato (core dump creato)

Then, is it possible to do this with xbot2?

EDIT
Backtrace of the error

#0  0x00007ffff657f438 in __GI_raise (sig=sig@entry=6)
    at ../sysdeps/unix/sysv/linux/raise.c:54
#1  0x00007ffff658103a in __GI_abort () at abort.c:89
#2  0x00007ffff6bc5dde in ?? ()
   from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#3  0x00007ffff6bd1896 in ?? ()
   from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#4  0x00007ffff6bd1901 in std::terminate() ()
   from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#5  0x00007ffff6bd1b55 in __cxa_throw ()
   from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#6  0x00007fffe51aaa0a in XBot::RobotInterfaceROS::init_robot(XBot::ConfigOptions const&) ()
   from /opt/xbot/lib/libRobotInterfaceROS.so
#7  0x00007ffff71f9e8d in XBot::RobotInterface::init_internal(XBot::ConfigOptions const&) ()
   from /opt/xbot/lib/libXBotInterface.so.2.0.1
#8  0x00007ffff71e3aac in XBot::XBotInterface::init(XBot::Config--Type <RET> for more, q to quit, c to continue without paging--
Options const&) () from /opt/xbot/lib/libXBotInterface.so.2.0.1
#9  0x00007ffff71f90ec in XBot::RobotInterface::getRobot(XBot::ConfigOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) () from /opt/xbot/lib/libXBotInterface.so.2.0.1
#10 0x00007ffff71f9af3 in XBot::RobotInterface::getRobot(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, boost::any, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, boost::any> > > const>, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) () from /opt/xbot/lib/libXBotInterface.so.2.0.1
#11 0x00007fffe5bd313c in GravityCompensation::run (this=0x7fffd40f8c40)
    at /home/tori/xbot2/src/xbot2_examples/src/gravity_compensation/gravity_compensation.cpp:65

6 0x00007fffe51aaa0a in XBot::RobotInterfaceROS::init_robot(XBot::ConfigOptions const&) ()
from /opt/xbot/lib/libRobotInterfaceROS.so

Here should be the error: I check the code and init_robot is a pure virtual function, probably the no implementation causes this error, who should implement it?

Answer 4 · 2020-11-10T10:33:54.000Z

Other updates.
The error above seems solved (edit it may be related to #19?). Now there are not crashes issues.

I am always trying to control the robot from a non ControlPlugin . I did a main where I call:

std::string path_to_config_file = XBot::Utils::getXBotConfig();
auto robot = XBot::RobotInterface::getRobot(path_to_config_file);

The robot pointer seems to be attached to the gazebo simulation, because if I move the robot in Gazebo (applying a casual force on some link through the GUI) I can see the joint positions changes with robot->getJointPosition(q_pos)

The problem is that I can not move the robot at all. I am trying:

robot->setPositionReference(q) with all the control modes
(applied with robot->setControlMode(XBot::ControlMode::XXX());
robot->setVelocityReference(q) with velocity control mode
Copying the homing examples: hence using the setting the joint positions to a robot model, and then call
robot->setReferenceFrom(*model);
I tried to use XBot::RobotInterfaceXBot2Rt pointer instead XBot::RobotInterface pointer:
- If I use auto robot = std::dynamic_pointer_cast<XBot::RobotInterfaceXBot2Rt>( XBot::RobotInterface::getRobot(path_to_config_file));, as you did in the xbot2_wip code (xbot2_wip/src/rt_plugin/control_plugin.cpp), the cast FAILS, the robot pointer is empty
- If I use static cast version, pointer IS NOT empty, but I can still not move the robot as in the previous trials

(After all trials I do not forget to call robot->move())

I am still wondering if not using a ControlPlugin is not possible, or I am doing things wrong somewhere

EDIT I add the code, maybe it is easy to check for errors:

#include <XBotInterface/RobotInterface.h>
#include <xbot2/robot_interface/robot_interface_xbot_rt.h>
#include <XBotInterface/Utils.h>

int main()
{

    std::string path_to_config_file = XBot::Utils::getXBotConfig();
    auto robot = XBot::RobotInterface::getRobot(path_to_config_file); // returns a shared pointer to a robot object
    
    robot->setControlMode(XBot::ControlMode::Position());
    robot->sense();

    Eigen::VectorXd q_actual, q_wanted;
    robot->getJointPosition(q_actual);
    q_wanted = q_actual;
    q_wanted[2] = q_actual[2]+0.5;
    
    while ((q_wanted[2] - q_actual[2]) > 0.01) {
        robot->sense();
        robot->getJointPosition(q_actual);
        
        std::cout << "difference:\n" << q_wanted[2] - q_actual[2] << std::endl;
        
        robot->setPositionReference(q_wanted);
        robot->move();
        
        usleep(1000);
    }

    return 0;
}

Result : robot does not move
OUTPUT

difference:
0.494196
difference:
0.494196
...

When I move robot with something that works (like homing plugin) the difference changes, hence the state of the robot is correctly read.

Answer 5 · 2020-11-10T16:14:09.000Z

From an external ROS node, you can indeed use RobotInterface as you did. Only missing step is to enable the joint control via ros, which is done by the ros_control plugin. This is also mentioned in the documentation - it is very recent, incomplete, etc though

Answer 6 · 2020-11-10T16:35:26.000Z

Thanks! with rosservice call /xbotcore/ros_ctrl/switch 1 I solved

Answer 7 · 2020-12-02T14:42:05.000Z

As we figured out discussing with @torydebra and @alaurenzi what @torydebra was looking for here was a RobotInterfaceXBot2ROS, which we will implement to control a robot outside the ControlPlugin and using the ROS framework.