Implement a YARP based ROS publisher class in YarpUtilities library.

[prashanthr05]

This PR adds a class for publishing wrenches, joint states and transforms over ROS topics using YARP only software.
Inspiration was taken from human-dynamics-estimation/publishers.

The class internally contains a YARP based ROS node and a set of publishers which can be configured for publishing over certain topics and configuring connections to a YARP transform server in order to publish transforms.

Although the class might be ROS independent, in order to run the code, ROS is required and usual YARP-ROS connections need to be made.

A test device has also been implemented, I can include it in this PR if required.

This PR depends on #155 if the test device needs to be added.

[prashanthr05]

cc @Yeshasvitvs

[prashanthr05]

An example usage of the configuration for the RosPublisher class in YARP device format is as follows,
This type of initialization was chosen thanks to @Yeshasvitvs advice, to be extensible to most ROS topic scenarios.

<?xml version="1.0" encoding="UTF-8" ?>
<device  xmlns:xi="http://www.w3.org/2001/XInclude" name="ros-publisher-test" type="ROSPublisherTestDevice">
    <param name="joint_states_topic">/joint_states</param>
    <param name="transform_server_port">/tfServer</param>
     <group name="WrenchPublishers">
        <param name="frame_names">("right", "left")</param> 
        <param name="topics">("/wrench/right", "/wrench/left")</param>
    </group>
</device>

Usage in source code,
In open(),

pub = std::make_unique<BipedalLocomotion::YarpUtilities::RosPublisher>("/PublisherTest");

In run()

   std::vector<std::string> jointList{"hello"};
   std::vector<double> jointPos{20.0};
   pub->publishJointStates(jointList, jointPos);
   
   std::vector<double> wrench{0.0, 1.0, 2.0,0.0, 0.0, 0.0};
   pub->publishWrench("right", wrench);
   
   
   std::vector<double> pose{1., 0, 0, 0, 0, 1., 0, 0, 0, 0, 1., 0, 0, 0, 0, 1.};
   pub->publishTransform("/world", "/dummy", pose);

For running requires,

• roscore,
• yarpserver --ros and
• yarpdev --device transformServer --ROS::enable_ros_publisher true --ROS::enable_ros_subscriber true --name tfServer

I tested proper working of joint states and wrenches, but the transform servers don't seem to be working as expected. I am debugging. (The code seems to be correct, though. updated in: #156 (comment)).

[DanielePucci]

CC @dic-iit/dynamic-interaction-control

[prashanthr05]

but the transform servers don't seem to be working as expected. I am debugging. (The code seems to be correct, though).

Seems to be working fine now. Looks like it was a local YARP configuration problem.

[S-Dafarra]

If I understood well, the actual time should be

timeStamp = sec_part + nsec_part/1e-9

so why not exploiting chrono also for that? You could subtract the number of seconds from the initial time and then convert it to nanoseconds

uint64_t sec_part  = std::chrono::duration_cast<std::chrono::seconds>(timeStamp).count();
auto nanosec_chrono =   std::chrono::duration_cast<std::chrono::seconds>(timeStamp - sec_part);
uint64_t nsec_part = std::chrono::duration_cast<std::chrono::nanoseconds>(nanosec_chrono).count();

I am not sure if this works, neither if it is correct, but I would like to avoid the appearance of weird 1000000000UL that are super error-prone.

[prashanthr05]

but I would like to avoid the appearance of weird 1000000000UL that are super error-prone.

Agreed. I have fixed the timestamp processing in 5a4c6b5 to avoid magic numbers.

[S-Dafarra]

Just to understand, do you have any test we can keep?

[prashanthr05]

Just to understand, do you have any test we can keep?

I have a test device in https://github.com/prashanthr05/bipedal-locomotion-framework/tree/device/tes-ros-publisher/devices/ROSPublisherTestDevice that I used to verify the working of this class.

I used that approach because it was on top of my mind at the time of implementation. But I suppose, it can be converted to a standard C++ application as well.

[S-Dafarra]

Just to understand, do you have any test we can keep?

I have a test device in https://github.com/prashanthr05/bipedal-locomotion-framework/tree/device/tes-ros-publisher/devices/ROSPublisherTestDevice that I used to verify the working of this class.

I used that approach because it was on top of my mind at the time of implementation. But I suppose, it can be converted to a standard C++ application as well.

Yeah, it would good to have some sort of testing, eventually using also bash scripts, but this can also go in a separate PR.

[prashanthr05]

Just to understand, do you have any test we can keep?

I have a test device in https://github.com/prashanthr05/bipedal-locomotion-framework/tree/device/tes-ros-publisher/devices/ROSPublisherTestDevice that I used to verify the working of this class.
I used that approach because it was on top of my mind at the time of implementation. But I suppose, it can be converted to a standard C++ application as well.

Yeah, it would good to have some sort of testing, eventually using also bash scripts, but this can also go in a separate PR.

I can open a new PR with the device, runner yarpmanager application(for running the prerequisites), and the commands to check the expected outputs. Eventually we can convert it to a test format we like.

[S-Dafarra]

Just to understand, do you have any test we can keep?

I have a test device in https://github.com/prashanthr05/bipedal-locomotion-framework/tree/device/tes-ros-publisher/devices/ROSPublisherTestDevice that I used to verify the working of this class.
I used that approach because it was on top of my mind at the time of implementation. But I suppose, it can be converted to a standard C++ application as well.

Yeah, it would good to have some sort of testing, eventually using also bash scripts, but this can also go in a separate PR.

I can open a new PR with the device, runner yarpmanager application(for running the prerequisites), and the commands to check the expected outputs. Eventually we can convert it to a test format we like.

Sure! As @traversaro mentioned, "if you like a feature, put a test on it" 😁

[prashanthr05]

Just to understand, do you have any test we can keep?

I have a test device in https://github.com/prashanthr05/bipedal-locomotion-framework/tree/device/tes-ros-publisher/devices/ROSPublisherTestDevice that I used to verify the working of this class.
I used that approach because it was on top of my mind at the time of implementation. But I suppose, it can be converted to a standard C++ application as well.

Yeah, it would good to have some sort of testing, eventually using also bash scripts, but this can also go in a separate PR.

I can open a new PR with the device, runner yarpmanager application(for running the prerequisites), and the commands to check the expected outputs. Eventually we can convert it to a test format we like.

Sure! As @traversaro mentioned, "if you like a feature, put a test on it"

A test device has been added in #160.

[prashanthr05]

@GiulioRomualdi if you have any comments for this PR, let me know.
@S-Dafarra otherwise, are we good for the merge?

[S-Dafarra]

@GiulioRomualdi if you have any comments for this PR, let me know.
@S-Dafarra otherwise, are we good for the merge?

Ok for me

[GiulioRomualdi]

Merging! Thank you @prashanthr05