C++, ROS to ROS2 keeping compatibility for service callbacks

Question

I've been working on a framework on top of ROS for the last couple of years and now I'm trying to make it compatible with both ROS and ROS2. For now I fixed the CMAKE the namespaces and the headears for messages, services and actions.

Now I need to work on the service callbacks.

Migration guide from ROS1 to ROS2

This is the guide that shows how it should be done in ROS2 compared to ROS1

// In ROS 1 it goes something like this
#include "nav_msgs/GetMap.h"

bool service_callback(
    nav_msgs::GetMap::Request & request,
    nav_msgs::GetMap::Response & response)
{
    /* if failed throw an exception (or return false) */
    /* if success */
    return true;
}

// In ROS 2 it should go like this
#include "nav_msgs/srv/get_map.hpp"  // This has been fixed

void service_callback(
    const std::shared_ptr<nav_msgs::srv::GetMap::Request> request,
    std::shared_ptr<nav_msgs::srv::GetMap::Response> response)
{
    /* if failed just throw an exception */
    /* if success */
    return;
}

---

The problem is that the ROS1 solution uses reference to the objects for both request and response, i looked at the available overloads for the callback and there are none that allowes you to use shared pointers instead. ROS2 uses shared pointers where the request is const and the response is not.

I already have my method that let me get instantiate a service with callback:

/*-----------------------------------------------------------------------------------------
 : GET SERVER : Returns a Server object
 *-----------------------------------------------------------------------------------------
 return a service server with normalized methods
 - server   : the server object to store the advertised service into
 - key      : the name of the topic (regardless of its namespace or node)
 - callback : the callback for the advertised service, an object method that takes a reference of both request and response
 - that     : pointer to the object that will receive messages through its callback
 - node     : the node or namespace this publisher is declared into
 return "true" if the serviceServer has been instantiated correctly, "false" otherwise
 *-----------------------------------------------------------------------------------------
 e.g.
 (key="data", node="") -> the publisher will be initialized in the handler namespace: /node-name/data
 (key="/data", node="") -> the publisher will be initialized in the root: /data
 (key=<any>, node="node-name") -> the publisher will be initialized in the specified node namespace: /node-name-namespace/<any>
 */
template <typename MReq, typename MRes, class N>
inline bool getServer(Server& server, string key, bool(N::*callback)(MReq &, MRes &), N* that,
                        const string& node=string()) {
    try {
        if (node.length() > 0 && !this->addNamespace(node, key)) return false;
        lock_guard<boost::mutex> lock(this->get_mutex());
        AdvertiseServiceOptions ops;
        ops.template init<MReq, MRes>(key, bind(callback, that, _1, _2));
        server = Server(key, this->advertiseService(ops));
        return true;
    } catch (...) {
        tgo::Exception exc = tgo::getException( std::current_exception() );
        std::cout << "GET SERVER : " << node << "/" << key << " : " << exc.what() << std::endl;
        return false;
    }
};

Where the Server class extends ServiceServer from ROS1.

Is there a workaround you may think of?

Since in this method I use a bind, I tried to convert the two placeholders to MReq and MRes respectively but the compiler doesn't seem to like it...

ops.template init<MReq, MRes>(key, bind(callback, that, make_shared<MReq>(_1), make_shared<MRes>(_2)));

it gives me this error

/usr/include/c++/11/ext/new_allocator.h: In instantiation of ‘void __gnu_cxx::new_allocator<_Tp>::construct(_Up*, _Args&& ...) [with _Up = double; _Args = {const std::_Placeholder<1>&}; _Tp = double]’:
/usr/include/c++/11/bits/alloc_traits.h:516:17:   required from ‘static void std::allocator_traits<std::allocator<_CharT> >::construct(std::allocator_traits<std::allocator<_CharT> >::allocator_type&, _Up*, _Args&& ...) [with _Up = double; _Args = {const std::_Placeholder<1>&}; _Tp = double; std::allocator_traits<std::allocator<_CharT> >::allocator_type = std::allocator<double>]’
/usr/include/c++/11/bits/shared_ptr_base.h:519:39:   required from ‘std::_Sp_counted_ptr_inplace<_Tp, _Alloc, _Lp>::_Sp_counted_ptr_inplace(_Alloc, _Args&& ...) [with _Args = {const std::_Placeholder<1>&}; _Tp = double; _Alloc = std::allocator<double>; __gnu_cxx::_Lock_policy _Lp = __gnu_cxx::_S_atomic]’
/usr/include/c++/11/bits/shared_ptr_base.h:650:16:   required from ‘std::__shared_count<_Lp>::__shared_count(_Tp*&, std::_Sp_alloc_shared_tag<_Alloc>, _Args&& ...) [with _Tp = double; _Alloc = std::allocator<double>; _Args = {const std::_Placeholder<1>&}; __gnu_cxx::_Lock_policy _Lp = __gnu_cxx::_S_atomic]’
/usr/include/c++/11/bits/shared_ptr_base.h:1342:14:   required from ‘std::__shared_ptr<_Tp, _Lp>::__shared_ptr(std::_Sp_alloc_shared_tag<_Tp>, _Args&& ...) [with _Alloc = std::allocator<double>; _Args = {const std::_Placeholder<1>&}; _Tp = double; __gnu_cxx::_Lock_policy _Lp = __gnu_cxx::_S_atomic]’
/usr/include/c++/11/bits/shared_ptr.h:409:59:   required from ‘std::shared_ptr<_Tp>::shared_ptr(std::_Sp_alloc_shared_tag<_Tp>, _Args&& ...) [with _Alloc = std::allocator<double>; _Args = {const std::_Placeholder<1>&}; _Tp = double]’
/usr/include/c++/11/bits/shared_ptr.h:862:14:   required from ‘std::shared_ptr<_Tp> std::allocate_shared(const _Alloc&, _Args&& ...) [with _Tp = double; _Alloc = std::allocator<double>; _Args = {const std::_Placeholder<1>&}]’
/usr/include/c++/11/bits/shared_ptr.h:878:39:   required from ‘std::shared_ptr<_Tp> std::make_shared(_Args&& ...) [with _Tp = double; _Args = {const std::_Placeholder<1>&}]’
main.cpp:29:58:   required from here
/usr/include/c++/11/ext/new_allocator.h:162:11: error: cannot convert ‘const std::_Placeholder<1>’ to ‘double’ in initialization
  162 |         { ::new((void *)__p) _Up(std::forward<_Args>(__args)...); }
      |           ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

I hoped to start using shared pointers instead of references here on and make ROS1 as well use them exploiting the bind object.

I would be ok using something like this:

template <typename MReq, typename MRes, class N>
inline bool getServer(Server& server, string key,
                      bool(N::*callback)(const shared_ptr<MReq>, shared_ptr<MRes>), N* that,
                      const string& node=string()) {
    try {
        if (node.length() > 0 && !this->addNamespace(node, key)) return false;
        lock_guard<boost::mutex> lock(this->get_mutex());
        AdvertiseServiceOptions ops;
        auto binded_method = bind(callback, that, make_shared<MReq>(_1), make_shared<MRes>(_2));
        ops.template init<MReq, MRes>(key, binded_method);
        server = Server(key, this->advertiseService(ops));
        return true;
    } catch (...) {
        tgo::Exception exc = tgo::getException( std::current_exception() );
        std::cout << "GET SERVER : " << node << "/" << key << " : " << exc.what() << std::endl;
        return false;
    }
};

---

ANSWER

I'm checking for lambdas but since this is a templated function I'm not sure it's going to work.

ate <typename MReq, typename MRes, class N>
inline bool getServer(Server& server, string key, bool(N::*callback)(MReq &, MRes &), N* that,
                        const string& node=string()) {
    try {
        if (node.length() > 0 && !this->addNamespace(node, key)) return false;
        lock_guard<boost::mutex> lock(this->get_mutex());
        AdvertiseServiceOptions ops;
        ops.template init<MReq, MRes>(key,
            // bind(callback, that, _1, _2)
            [that,callback,&server](MReq& req, MReq& res) mutable -> bool {
                try {
                    (that->*callback)(req,res); return true;
                } catch (...) {
                    tgo::Exception exc = tgo::getException( std::current_exception() );
                    server.setState( tgo::TgoCode::FATAL, exc.what() ); throw exc;
                }
            }
        );
        server = Server(key, this->advertiseService(ops));
        return true;
    } catch (...) {
        tgo::Exception exc = tgo::getException( std::current_exception() );
        std::cout << "GET SERVER : " << node << "/" << key << " : " << exc.what() << std::endl;
        return false;
    }
};

This compiles and run without exceptions used like so:

// Only the definitions, not the declarations involved, it would be too long

// Where i instantiate the callback
bool Node::init_servers() {
    bool success = this->handler_.getServer( this->server_reboot, "reboot", &Node::server_reboot_cb, this );
    this->servers.add(this->server_reboot);
    return success;
}

// Where i define the callback
void Node::server_reboot_cb ( const shared_ptr<std_srvs::srv::SetBool::Request> request,
                                                shared_ptr<std_srvs::srv::SetBool::Response> response ) {
    response->success = request->data ? this->reboot() : true;
}

I even had a chance to simplify the callbacks removing a try catch from it, moving it in the lambda: I use it to keep trace of every exception that occurs.

---

FOLLOWUP

Some questions remains:

Usually I use factories to create multiple typed instances of templated classes that uses callbacks for both topics and services later emplaced in vectors or maps, in these cases does the lambda captures work?

Since today I had no problem using boost::bind or std::bind.

For what I understood bind evaluates the attributes when it run, not when it's created.

Lambda on the other hand evaluates captures when it's created and not when it run.

Did I got it right? 'cause if I did I doubt I'll be able to replace binds with lambdas.

Other question is: how do lambdas perform compared to binds? what's the catch in using them over bind?