コード例 #1
0
ファイル: analogServer.cpp プロジェクト: apaikan/icub-main
bool AnalogServerHandler::read(yarp::os::ConnectionReader& connection)
{
    yarp::os::Bottle in;
    yarp::os::Bottle out;
    bool ok=in.read(connection);
    if (!ok) return false;

    // parse in, prepare out
    int code = in.get(0).asVocab();
    bool ret=false;
    if (code==VOCAB_IANALOG)
    {
        ret=_handleIAnalog(in, out);
    }

    if (!ret)
    {
        out.clear();
        out.addVocab(VOCAB_FAILED);
    }

    yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
    if (returnToSender!=NULL) {
        out.write(*returnToSender);
    }
    return true;
}
コード例 #2
0
ファイル: BatteryWrapper.cpp プロジェクト: AbuMussabRaja/yarp
bool BatteryWrapper::read(yarp::os::ConnectionReader& connection)
{
    yarp::os::Bottle in;
    yarp::os::Bottle out;
    bool ok = in.read(connection);
    if (!ok) return false;

    // parse in, prepare out
    int code = in.get(0).asVocab();
    bool ret = false;
    if (code == VOCAB_IBATTERY)
    {
        int cmd = in.get(1).asVocab();
        if (cmd == VOCAB_BATTERY_INFO)
        {
            if (battery_p)
            {
                yarp::os::ConstString info;
                battery_p->getBatteryInfo(info);
                out.addVocab(VOCAB_IS);
                out.addVocab(cmd);
                out.addString(info);
                ret = true;
            }
        }
        else
        {
            yError("Invalid vocab received in BatteryWrapper");
        }
    }
    else
    {
        yError("Invalid vocab received in BatteryWrapper");
    }

    if (!ret)
    {
        out.clear();
        out.addVocab(VOCAB_FAILED);
    }

    yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
    if (returnToSender != NULL) {
        out.write(*returnToSender);
    }
    return true;
}
コード例 #3
0
bool TaskYarpInterface::RpcMessageCallback::read(yarp::os::ConnectionReader& connection)
{
    yarp::os::Bottle input;
    yarp::os::Bottle reply;

    if (!input.read(connection)){
        return false;
    }
    else{
        tmBase.parseIncomingMessage(input, reply);
        yarp::os::ConnectionWriter* returnToSender = connection.getWriter();
        if (returnToSender!=NULL) {
            if (!reply.write(*returnToSender)) {
                OCRA_ERROR("Reply was not successfully written");
                return false;
            }
        }
        return true;
    }
}
コード例 #4
0
bool RGBDSensorWrapper::read(yarp::os::ConnectionReader& connection)
{
    yarp::os::Bottle in;
    yarp::os::Bottle out;
    bool ok = in.read(connection);
    if (!ok) return false;

    // parse in, prepare out
//     int action = in.get(0).asVocab();
//     int inter  = in.get(1).asVocab();
    bool ret = false;

    if (!ret)
    {
        out.clear();
        out.addVocab(VOCAB_FAILED);
    }

    yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
    if (returnToSender != NULL) {
        out.write(*returnToSender);
    }
    return true;
}
コード例 #5
0
bool TeoXRpcResponder::read(yarp::os::ConnectionReader& connection) {
    yarp::os::Bottle in, out;
    in.read(connection);
    printf("[xRpcResponder] Got %s\n", in.toString().c_str());
    out.clear();
    yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
    if (returnToSender==NULL) return false;
    if ((in.get(0).asString() == "help")||(in.get(0).asVocab() == VOCAB_HELP))  // help //
    {
        out.addString("Available commands: [help] [load] [stat]");
        return out.write(*returnToSender);
    }
    else if ((in.get(0).asString() == "load")||(in.get(0).asVocab() == VOCAB_LOAD))  // load //
    {
        if ( in.size() != 2 )
        {
            CD_ERROR("in.size() != 2\n");
            out.addVocab(VOCAB_FAILED);
            return out.write(*returnToSender);
        }
        if( ! cartesianRateThread->load( in.get(1).asString() ) )
        {
            CD_ERROR("cartesianRateThread->load failed\n");
            out.addVocab(VOCAB_FAILED);
            return out.write(*returnToSender);
        }
        out.addVocab(VOCAB_OK);
        return out.write(*returnToSender);
    }
    else if ((in.get(0).asString() == "stat")||(in.get(0).asVocab() == VOCAB_STAT))  // stat //
    {
        std::vector<double> stat;
        if( ! cartesianRateThread->stat(stat) )
            out.addVocab(VOCAB_FAILED);
        else
            for(int i=0;i<stat.size();i++)
                out.addDouble(stat[i]);
        return out.write(*returnToSender);
    }
    else if  ((in.get(0).asString() == "inv")||(in.get(0).asVocab() == VOCAB_INV))  // inv //
    {
        std::vector<double> xd, q;
        for(int i=1;i<in.size();i++)
            xd.push_back(in.get(i).asDouble());
        if( ! cartesianRateThread->inv(xd,q) )
            out.addVocab(VOCAB_FAILED);
        else
            for(int i=0;i<q.size();i++)
                out.addDouble(q[i]);
        return out.write(*returnToSender);
    }
    else if  ((in.get(0).asString() == "movj")||(in.get(0).asVocab() == VOCAB_MOVJ))  // movj //
    {
        std::vector<double> xd, q;
        for(int i=1;i<in.size();i++)
            xd.push_back(in.get(i).asDouble());
        if( ! cartesianRateThread->movj(xd) )
            out.addVocab(VOCAB_FAILED);
        else
        {
            if(in.check("wait"))
            {
                CD_SUCCESS("Waiting\n");
                bool done = false;
                while(!done) {
                    cartesianRateThread->checkMotionDone(&done);
                    printf(".");
                    fflush(stdout);
                    yarp::os::Time::delay(0.5);
                }
                printf("\n");
            }
            out.addVocab(VOCAB_OK);
        }
        return out.write(*returnToSender);
    }
    else
    {
        fprintf(stderr,"[xRpcResponder] fail: Unknown command (use 'help' if needed).\n");
        out.addVocab(VOCAB_FAILED);
        return out.write(*returnToSender);
    }
}
コード例 #6
0
bool Rangefinder2DWrapper::read(yarp::os::ConnectionReader& connection)
{
    yarp::os::Bottle in;
    yarp::os::Bottle out;
    bool ok = in.read(connection);
    if (!ok) return false;

    // parse in, prepare out
    int action = in.get(0).asVocab();
    int inter  = in.get(1).asVocab();
    bool ret = false;

    if (inter == VOCAB_ILASER2D)
    {
        if (action == VOCAB_GET)
        {
            int cmd = in.get(2).asVocab();
            if (cmd == VOCAB_DEVICE_INFO)
            {
                if (sens_p)
                {
                    yarp::os::ConstString info;
                    if (sens_p->getDeviceInfo(info))
                    {
                        out.addVocab(VOCAB_IS);
                        out.addVocab(cmd);
                        out.addString(info);
                        ret = true;
                    }
                    else
                    {
                        ret = false;
                    }
                }
            }
            else if (cmd == VOCAB_LASER_DISTANCE_RANGE)
            {
                if (sens_p)
                {
                    double max = 0;
                    double min = 0;
                    if (sens_p->getDistanceRange(min, max))
                    {
                        out.addVocab(VOCAB_IS);
                        out.addVocab(cmd);
                        out.addDouble(min);
                        out.addDouble(max);
                        ret = true;
                    }
                    else
                    {
                        ret = false;
                    }
                }
            }
            else if (cmd == VOCAB_LASER_ANGULAR_RANGE)
            {
                if (sens_p)
                {
                    double max = 0;
                    double min = 0;
                    if (sens_p->getScanLimits(min, max))
                    {
                        out.addVocab(VOCAB_IS);
                        out.addVocab(cmd);
                        out.addDouble(min);
                        out.addDouble(max);
                        ret = true;
                    }
                    else
                    {
                        ret = false;
                    }
                }
            }
            else if (cmd == VOCAB_LASER_ANGULAR_STEP)
            {
                if (sens_p)
                {
                    double step = 0;
                    if (sens_p->getHorizontalResolution(step))
                    {
                        out.addVocab(VOCAB_IS);
                        out.addVocab(cmd);
                        out.addDouble(step);
                        ret = true;
                    }
                    else
                    {
                        ret = false;
                    }
                }
            }
            else if (cmd == VOCAB_LASER_SCAN_RATE)
            {
                if (sens_p)
                {
                    double rate = 0;
                    if (sens_p->getScanRate(rate))
                    {
                        out.addVocab(VOCAB_IS);
                        out.addVocab(cmd);
                        out.addDouble(rate);
                        ret = true;
                    }
                    else
                    {
                        ret = false;
                    }
                }
            }
            else
            {
                yError("Invalid command received in Rangefinder2DWrapper");
            }
        }
        else if (action == VOCAB_SET)
        {
            int cmd = in.get(2).asVocab();
            if (cmd == VOCAB_LASER_DISTANCE_RANGE)
            {
                if (sens_p)
                {
                    double min = in.get(3).asInt();
                    double max = in.get(4).asInt();
                    sens_p->setDistanceRange(min, max);
                    ret = true;
                }
            }
            else if (cmd == VOCAB_LASER_ANGULAR_RANGE)
            {
                if (sens_p)
                {
                    double min = in.get(3).asInt();
                    double max = in.get(4).asInt();
                    sens_p->setScanLimits(min, max);
                    ret = true;
                }
            }
            else if (cmd == VOCAB_LASER_SCAN_RATE)
            {
                if (sens_p)
                {
                    double rate = in.get(3).asInt();
                    sens_p->setScanRate(rate);
                    ret = true;
                }
            }
            else if (cmd == VOCAB_LASER_ANGULAR_STEP)
            {
                if (sens_p)
                {
                    double step = in.get(3).asDouble();
                    sens_p->setHorizontalResolution(step);
                    ret = true;
                }
            }
            else
            {
                yError("Invalid command received in Rangefinder2DWrapper");
            }
        }
        else
        {
            yError("Invalid action received in Rangefinder2DWrapper");
        }
    }
    else
    {
        yError("Invalid interface vocab received in Rangefinder2DWrapper");
    }

    if (!ret)
    {
        out.clear();
        out.addVocab(VOCAB_FAILED);
    }

    yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
    if (returnToSender != NULL) {
        out.write(*returnToSender);
    }
    return true;
}
コード例 #7
0
ファイル: LocationsServer.cpp プロジェクト: apaikan/yarp
bool yarp::dev::LocationsServer::read(yarp::os::ConnectionReader& connection)
{
    yarp::os::Bottle in;
    yarp::os::Bottle out;
    bool             ret;
    int              code;

    bool ok = in.read(connection);
    if (!ok) return false;

    // parse in, prepare out
    if(in.get(0).isString())
    {
        if (in.get(0).asString() == "save" && in.get(1).isString())
        {
            if(save_locations(in.get(1).asString()))
            {
                out.addString(in.get(1).asString() + " successfully saved");
            }
        }
        else if (in.get(0).asString() == "load" && in.get(1).isString())
        {
            if(load_locations(in.get(1).asString()))
            {
                out.addString(in.get(1).asString() + " successfully loaded");
            }
        }
        else if(in.get(0).asString() == "list")
        {
            std::map<std::string, Map2DLocation>::iterator it;
            for (it = m_locations.begin(); it != m_locations.end(); it++)
            {
                out.addString(it->first);
            }
        }
        else if(in.get(0).asString() == "help")
        {
            out.addString("'save <full path filename>' to save locations on a file");
            out.addString("'load <full path filename>' to load locations from a file");
            out.addString("'list' to view locations stored");
        }
        else
        {
            out.addString("request not undestood, call 'help' to see a list of avaiable commands");
        }

        yarp::os::ConnectionWriter *returnToSender = connection.getWriter();

        if (returnToSender != NULL)
        {
            out.write(*returnToSender);
        }

        updateVizMarkers();
        return true;
    }
    code = in.get(0).asVocab();
    ret  = false;

    if (code == VOCAB_INAVIGATION)
    {
        int cmd = in.get(1).asVocab();
        if (cmd == VOCAB_NAV_GET_LOCATION_LIST)
        {
            yarp::os::ConstString info;

            out.addVocab(VOCAB_OK);
            Bottle& l = out.addList();

            std::map<std::string, Map2DLocation>::iterator it;
            for (it = m_locations.begin(); it != m_locations.end(); it++)
            {
                l.addString(it->first);
            }

            ret = true;
        }
        else if (cmd == VOCAB_NAV_CLEAR)
        {
            m_locations.clear();
            out.addVocab(VOCAB_OK);
            ret = true;
        }
        else if (cmd == VOCAB_NAV_DELETE)
        {
            std::string name = in.get(2).asString();

            std::map<std::string, Map2DLocation>::iterator it;
            it = m_locations.find(name);
            if (it != m_locations.end())
            {
                m_locations.erase(it);
                out.addVocab(VOCAB_OK);
            }
            else
            {
                yError("User requested an invalid location name");
                out.addVocab(VOCAB_ERR);
            }

            ret = true;
        }
        else if (cmd == VOCAB_NAV_GET_LOCATION)
        {
            std::string name = in.get(2).asString();

            std::map<std::string, Map2DLocation>::iterator it;
            it = m_locations.find(name);
            if (it != m_locations.end())
            {
                out.addVocab(VOCAB_OK);
                Map2DLocation loc = it->second;
                out.addString(loc.map_id);
                out.addDouble(loc.x);
                out.addDouble(loc.y);
                out.addDouble(loc.theta);
            }
            else
            {
                out.addVocab(VOCAB_ERR);
                yError("User requested an invalid location name");
            }

            ret = true;
        }
        else if (cmd == VOCAB_NAV_STORE_ABS)
        {
            Map2DLocation         location;
            yarp::os::ConstString name = in.get(2).asString();

            location.map_id = in.get(3).asString();
            location.x      = in.get(4).asDouble();
            location.y      = in.get(5).asDouble();
            location.theta  = in.get(6).asDouble();

            m_locations.insert(std::pair<std::string, Map2DLocation>(name, location));
            out.addVocab(VOCAB_OK);
            ret = true;
        }
        else
        {
            yError("Invalid vocab received in LocationsServer");
        }
    }
    else
    {
        yError("Invalid vocab received in LocationsServer");
    }

    if (!ret)
    {
        out.clear();
        out.addVocab(VOCAB_FAILED);
    }

    yarp::os::ConnectionWriter *returnToSender = connection.getWriter();

    if (returnToSender != NULL)
    {
        out.write(*returnToSender);
    }

    updateVizMarkers();
    return true;
}
コード例 #8
0
//------------------------------------------------------------------------------------------------------------------------------
bool yarp::dev::FrameTransformClient::read(yarp::os::ConnectionReader& connection)
{
    LockGuard lock (m_rpc_mutex);
    yarp::os::Bottle in;
    yarp::os::Bottle out;
    bool ok = in.read(connection);
    if (!ok) return false;

    std::string request = in.get(0).asString();
    if (request == "help")
    {
        out.addVocab(Vocab::encode("many"));
        out.addString("'get_transform <src> <dst>: print the transform from <src> to <dst>");
        out.addString("'list_frames: print all the available refence frames");
        out.addString("'list_ports: print all the opened ports for tranform broadcasting");
        out.addString("'publish_transform <src> <dst> <portname> <format>: opens a port to publish transform from src to dst");
        out.addString("'unpublish_transform <portname>: closes a previously opened port to publish a transform");
        out.addString("'unpublish_all <portname>: closes a all previously opened ports to publish a transform");
    }
    else if (request == "list_frames")
    {
        std::vector<std::string> v;
        this->getAllFrameIds(v);
        out.addVocab(Vocab::encode("many"));
        out.addString("List of available reference frames:");
        int count = 0;
        for (auto vec = v.begin(); vec != v.end(); vec++)
        {
            count++;
            std::string str = std::to_string(count) + "- " + *vec;
            out.addString(str.c_str());
        }
    }
    else if (request == "get_transform")
    {
        std::string src = in.get(1).asString();
        std::string dst = in.get(2).asString();
        out.addVocab(Vocab::encode("many"));
        yarp::sig::Matrix m;
        this->getTransform(src, dst, m);
        out.addString("Tranform from " + src + " to " + dst + " is: ");
        out.addString(m.toString());
    }
    else if (request == "list_ports")
    {
        out.addVocab(Vocab::encode("many"));
        if (m_array_of_ports.size()==0)
        {
            out.addString("No ports are currently active");
        }
        for (auto it = m_array_of_ports.begin(); it != m_array_of_ports.end(); it++)
        {
            if (*it)
            {
                std::string  s = (*it)->port.getName() + " "+ (*it)->transform_src +  " -> " + (*it)->transform_dst;
                out.addString(s);
            }
        }
    }
    else if (request == "publish_transform")
    {
        out.addVocab(Vocab::encode("many"));
        std::string src  = in.get(1).asString();
        std::string dst  = in.get(2).asString();
        std::string port_name = in.get(3).asString();
        std::string format = "matrix";
        if (in.size() > 4)
            {format= in.get(4).asString();}
        if (port_name[0]=='/')  port_name.erase(port_name.begin());
        std::string full_port_name = m_local_name + "/" + port_name;
        bool ret = true;
        for (auto it = m_array_of_ports.begin(); it != m_array_of_ports.end(); it++)
        {
            if ((*it) && (*it)->port.getName() == full_port_name)
            {
                ret = false;
                break;
            }
        }
        if (this->frameExists(src)==false)
        {
            out.addString("Requested src frame " + src + " does not exists.");
            yWarning("Requested src frame %s does not exists.", src.c_str());
        }
        if (this->frameExists(dst)==false)
        {
            out.addString("Requested dst frame " + dst + " does not exists.");
            yWarning("Requested fst frame %s does not exists.", dst.c_str());
        }
        if (ret == true)
        {
            broadcast_port_t* b = new broadcast_port_t;
            b->transform_src = src;
            b->transform_dst = dst;
            b->format = format;
            bool pret = b->port.open(full_port_name);
            if (pret)
            {
                out.addString("Operation complete. Port " + full_port_name + " opened.");
                m_array_of_ports.push_back(b);
                if (m_array_of_ports.size()==1) this->start();
            }
            else
            {
                delete b;
                out.addString("Operation failed. Unable to open port " + full_port_name + ".");
            }
        }
        else
        {
            out.addString("unable to perform operation");
        }
    }
    else if (request == "unpublish_all")
    {
        for (auto it = m_array_of_ports.begin(); it != m_array_of_ports.end(); it++)
        {
            (*it)->port.close();
            delete (*it);
            (*it)=nullptr;
        }
        m_array_of_ports.clear();
        if (m_array_of_ports.size()==0) this->askToStop();
        out.addString("Operation complete");
    }
    else if (request == "unpublish_transform")
    {
        bool ret = false;
        std::string port_name = in.get(1).asString();
        if (port_name[0]=='/')  port_name.erase(port_name.begin());
        std::string full_port_name = m_local_name + "/" + port_name;
        for (auto it = m_array_of_ports.begin(); it != m_array_of_ports.end(); it++)
        {
            if ((*it)->port.getName() == port_name)
            {
                (*it)->port.close();
                delete (*it);
                (*it)=nullptr;
                 m_array_of_ports.erase(it);
                 ret = true;
                 break;
            }
        }
        if (ret)
        {
            out.addString("Port " + full_port_name + " has been closed.");
        }
        else
        {
            out.addString("Port " + full_port_name + " was not found.");
        }
        if (m_array_of_ports.size()==0) this->askToStop();
    }
    else
    {
        yError("Invalid vocab received in FrameTransformClient");
        out.clear();
        out.addVocab(VOCAB_ERR);
        out.addString("Invalid command name");
    }

    yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
    if (returnToSender != nullptr)
    {
        out.write(*returnToSender);
    }
    else
    {
        yError() << "FrameTransformClient: invalid return to sender";
    }
    return true;
}
コード例 #9
0
bool FrameTransformServer::read(yarp::os::ConnectionReader& connection)
{
    LockGuard lock(m_mutex);
    yarp::os::Bottle in;
    yarp::os::Bottle out;
    bool ok = in.read(connection);
    if (!ok) return false;

    string request = in.get(0).asString();

    // parse in, prepare out
    int code = in.get(0).asVocab();
    bool ret = false;
    if (code == VOCAB_ITRANSFORM)
    {
        int cmd = in.get(1).asVocab();
        if (cmd == VOCAB_TRANSFORM_SET)
        {
            if (in.size() != 12)
            {
                yError() << "FrameTransformServer::read() protocol error";
                out.clear();
                out.addVocab(VOCAB_FAILED);
            }
            else
            {
                FrameTransform t;
                t.src_frame_id = in.get(2).asString();
                t.dst_frame_id = in.get(3).asString();
                double duration = in.get(4).asDouble();
                t.translation.tX = in.get(5).asDouble();
                t.translation.tY = in.get(6).asDouble();
                t.translation.tZ = in.get(7).asDouble();
                t.rotation.w() = in.get(8).asDouble();
                t.rotation.x() = in.get(9).asDouble();
                t.rotation.y() = in.get(10).asDouble();
                t.rotation.z() = in.get(11).asDouble();
                t.timestamp = yarp::os::Time::now();
                bool static_transform;
                if (duration > 0)
                {
                    static_transform = false;
                }
                else
                {
                    static_transform = true;
                }

                if (static_transform)
                {
                    ret = m_yarp_static_transform_storage->set_transform(t);
                }
                else
                {
                    ret = m_yarp_timed_transform_storage->set_transform(t);
                }
                if (ret == true)
                {
                    out.clear();
                    out.addVocab(VOCAB_OK);
                }
                else
                {
                    out.clear();
                    out.addVocab(VOCAB_FAILED);
                    yError() << "FrameTransformServer::read() something strange happened";
                }
            }
        }
        else if (cmd == VOCAB_TRANSFORM_DELETE_ALL)
        {
            m_yarp_timed_transform_storage->clear();
            m_yarp_static_transform_storage->clear();
            m_ros_timed_transform_storage->clear();
            m_ros_static_transform_storage->clear();
            out.clear();
            out.addVocab(VOCAB_OK);
        }
        else if (cmd == VOCAB_TRANSFORM_DELETE)
        {
            string frame1 = in.get(2).asString();
            string frame2 = in.get(3).asString();
            bool ret1 = m_yarp_timed_transform_storage->delete_transform(frame1, frame2);
            if (ret1 == true)
            {
                out.clear();
                out.addVocab(VOCAB_OK);
            }
            else
            {
                bool ret2 = m_yarp_static_transform_storage->delete_transform(frame1, frame2);
                if (ret2 == true)
                {
                    out.clear();
                    out.addVocab(VOCAB_OK);
                }
            }

        }
        else
        {
            yError("Invalid vocab received in FrameTransformServer");
            out.clear();
            out.addVocab(VOCAB_ERR);
        }
    }
    else if(request == "help")
    {
        out.addVocab(Vocab::encode("many"));
        out.addString("'list': get all transforms stored");
        out.addString("'delete_all': delete all transforms");
        out.addString("'set_static_transform <src> <dst> <x> <y> <z> <roll> <pitch> <yaw>': create a static transform");
    }
    else if (request == "set_static_transform")
    {
        FrameTransform t;
        t.src_frame_id = in.get(1).asString();
        t.dst_frame_id = in.get(2).asString();
        t.translation.tX = in.get(3).asDouble();
        t.translation.tY = in.get(4).asDouble();
        t.translation.tZ = in.get(5).asDouble();
        t.rotFromRPY(in.get(6).asDouble(), in.get(7).asDouble(), in.get(8).asDouble());
        t.timestamp = yarp::os::Time::now();
        bool static_transform = true;
        ret = m_yarp_static_transform_storage->set_transform(t);
        if (ret == true)
        {
            yInfo() << "set_static_transform done";
            out.addString("set_static_transform done");
        }
        else
        {
            yError() << "FrameTransformServer::read() something strange happened";
        }
    }
    else if(request == "delete_all")
    {
        m_yarp_timed_transform_storage->clear();
        m_yarp_static_transform_storage->clear();
        m_ros_timed_transform_storage->clear();
        m_ros_static_transform_storage->clear();
        yInfo() << "delete_all done";
        out.addString("delete_all done");
    }
    else if (request == "list")
    {
        out.addVocab(Vocab::encode("many"));
        list_response(out);
    }
    else
    {
        yError("Invalid vocab received in FrameTransformServer");
        out.clear();
        out.addVocab(VOCAB_ERR);
    }

    yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
    if (returnToSender != NULL)
    {
        out.write(*returnToSender);
    }
    else
    {
        yError() << "FrameTransformServer: invalid return to sender";
    }
    return true;
}