예제 #1
0
bool TcpRosCarrier::sendHeader(Protocol& proto) {
    dbg_printf("Route is %s\n", proto.getRoute().toString().c_str());
    Name n(proto.getRoute().getCarrierName() + "://test");
    String mode = "topic";
    String modeValue = n.getCarrierModifier("topic");
    if (modeValue=="") {
        mode = "service";
        modeValue = n.getCarrierModifier("service");
        isService = true;
    }
    if (modeValue=="") {
        printf("*** no topic or service specified!\n");
        mode = "topic";
        modeValue = "notopic";
        isService = false;
    }
    String rawValue = n.getCarrierModifier("raw");
#ifdef FORCE_ROS_NATIVE
    raw = 2;
#endif
    if (rawValue=="2") {
        raw = 2;
        dbg_printf("ROS-native mode requested\n");
    } else if (rawValue=="1") {
        raw = 1;
        dbg_printf("Raw mode requested\n");
    } else if (rawValue=="0") {
        raw = 0;
        dbg_printf("Cooked mode requested\n");
    }

    RosHeader header;
    dbg_printf("Writing to %s\n", proto.getStreams().getRemoteAddress().toString().c_str()); 
    dbg_printf("Writing from %s\n", proto.getStreams().getLocalAddress().toString().c_str());
    //header.data["type"] = "std_msgs/String";
    header.data[mode.c_str()] = modeValue.c_str();
    header.data["md5sum"] = "*";
    header.data["callerid"] = proto.getRoute().getFromName().c_str();
    //"/not/valid/at/all/i/am/afraid/old/chum";
    string header_serial = header.writeHeader();
    string header_len(4,'\0');
    char *at = (char*)header_len.c_str();
    RosHeader::appendInt(at,header_serial.length());
    dbg_printf("Writing %s -- %d bytes\n", 
               RosHeader::showMessage(header_len).c_str(),
               (int)header_len.length());

    Bytes b1((char*)header_len.c_str(),header_len.length());
    proto.os().write(b1);
    dbg_printf("Writing %s -- %d bytes\n", 
               RosHeader::showMessage(header_serial).c_str(),
               (int)header_serial.length());
    Bytes b2((char*)header_serial.c_str(),header_serial.length());
    proto.os().write(b2);

    return proto.os().isOk();
}
예제 #2
0
bool TcpRosCarrier::sendHeader(ConnectionState& proto) {
    dbg_printf("Route is %s\n", proto.getRoute().toString().c_str());
    Name n(proto.getRoute().getCarrierName() + "://test");
    ConstString mode = "topic";
    ConstString modeValue = n.getCarrierModifier("topic");
    if (modeValue=="") {
        mode = "service";
        modeValue = n.getCarrierModifier("service");
        isService = true;
    }
    if (modeValue=="") {
        printf("*** no topic or service specified!\n");
        mode = "topic";
        modeValue = "notopic";
        isService = false;
    }
    ConstString rawValue = n.getCarrierModifier("raw");
    if (rawValue=="2") {
        raw = 2;
        dbg_printf("ROS-native mode requested\n");
    } else if (rawValue=="1") {
        raw = 1;
        dbg_printf("Raw mode requested\n");
    } else if (rawValue=="0") {
        raw = 0;
        dbg_printf("Cooked mode requested\n");
    }

    RosHeader header;
    dbg_printf("Writing to %s\n", proto.getStreams().getRemoteAddress().toString().c_str()); 
    dbg_printf("Writing from %s\n", proto.getStreams().getLocalAddress().toString().c_str());

    ConstString rtyp = getRosType(proto);
    if (rtyp!="") {
        header.data["type"] = rtyp.c_str();
    }
    header.data[mode.c_str()] = modeValue.c_str();
    header.data["md5sum"] = (md5sum!="")?md5sum:"*";
    if (message_definition!="") {
        header.data["message_definition"] = message_definition;
    }
    NestedContact nc(proto.getRoute().getFromName());
    header.data["callerid"] = nc.getNodeName();
    header.data["persistent"] = "1";
    string header_serial = header.writeHeader();
    string header_len(4,'\0');
    char *at = (char*)header_len.c_str();
    RosHeader::appendInt(at,header_serial.length());
    dbg_printf("Writing %s -- %d bytes\n", 
               RosHeader::showMessage(header_len).c_str(),
               (int)header_len.length());

    Bytes b1((char*)header_len.c_str(),header_len.length());
    proto.os().write(b1);
    dbg_printf("Writing %s -- %d bytes\n", 
               RosHeader::showMessage(header_serial).c_str(),
               (int)header_serial.length());
    Bytes b2((char*)header_serial.c_str(),header_serial.length());
    proto.os().write(b2);

    return proto.os().isOk();
}
예제 #3
0
bool TcpRosCarrier::expectSenderSpecifier(Protocol& proto) {
    proto.setRoute(proto.getRoute().addFromName("tcpros"));

    dbg_printf("Trying for tcpros header\n");
    ManagedBytes m(headerLen1);
    Bytes mrem(m.get()+4,m.length()-4);
    NetInt32 ni = headerLen2;
    memcpy(m.get(),(char*)(&ni), 4);
    dbg_printf("reading %d bytes\n", (int)mrem.length());
    int res = NetType::readFull(proto.is(),mrem);
    dbg_printf("read %d bytes\n", res);
    if (res!=(int)mrem.length()) {
        printf("Fail %s %d\n", __FILE__, __LINE__);
        return false;
    }
    RosHeader header;
    header.readHeader(string(m.get(),m.length()));
    dbg_printf("Got header %s\n", header.toString().c_str());

    ConstString rosname = "";
    if (header.data.find("type")!=header.data.end()) {
        rosname = header.data["type"].c_str();
    }
    dbg_printf("<outgoing> Type of data is %s\n", rosname.c_str());
    kind = TcpRosStream::rosToKind(rosname.c_str()).c_str();
    dbg_printf("Loose translation [%s]\n", kind.c_str());
#ifdef FORCE_ROS_NATIVE
    kind = "";
#endif
    if (kind!="") {
        twiddler.configure(kind.c_str());
        translate = TCPROS_TRANSLATE_TWIDDLER;
    }

    if (header.data.find("callerid")!=header.data.end()) {
        proto.setRoute(proto.getRoute().addFromName(header.data["callerid"].c_str()));
    } else {
        proto.setRoute(proto.getRoute().addFromName("tcpros"));
    }

    // Let's just ignore everything that is sane and holy, and
    // send the same header right back.
    // Oh, ok, let's modify the callerid.  Begrudgingly.
    header.data["callerid"] = proto.getRoute().getToName().c_str();

    string header_serial = header.writeHeader();
    string header_len(4,'\0');
    char *at = (char*)header_len.c_str();
    RosHeader::appendInt(at,header_serial.length());
    dbg_printf("Writing %s -- %d bytes\n", 
               RosHeader::showMessage(header_len).c_str(),
               (int)header_len.length());
    
    Bytes b1((char*)header_len.c_str(),header_len.length());
    proto.os().write(b1);
    dbg_printf("Writing %s -- %d bytes\n", 
               RosHeader::showMessage(header_serial).c_str(),
               (int)header_serial.length());
    Bytes b2((char*)header_serial.c_str(),header_serial.length());
    proto.os().write(b2);

    return true;
}
예제 #4
0
bool TcpRosCarrier::expectSenderSpecifier(ConnectionState& proto) {
    proto.setRoute(proto.getRoute().addFromName("tcpros"));

    dbg_printf("Trying for tcpros header\n");
    ManagedBytes m(headerLen1);
    Bytes mrem(m.get()+4,m.length()-4);
    NetInt32 ni = headerLen2;
    memcpy(m.get(),(char*)(&ni), 4);
    dbg_printf("reading %d bytes\n", (int)mrem.length());
    int res = proto.is().readFull(mrem);
    dbg_printf("read %d bytes\n", res);
    if (res!=(int)mrem.length()) {
        if (res>=0) {
            fprintf(stderr,"TCPROS header failure, expected %d bytes, got %d bytes\n",
                    (int)mrem.length(),res);
        } else {
            fprintf(stderr,"TCPROS connection has gone terribly wrong\n");
        }
        return false;
    }
    RosHeader header;
    header.readHeader(string(m.get(),m.length()));
    dbg_printf("Got header %s\n", header.toString().c_str());

    ConstString rosname = "";
    if (header.data.find("type")!=header.data.end()) {
        rosname = header.data["type"].c_str();
    }
    ConstString rtyp = getRosType(proto);
    if (rtyp!="") {
        rosname = rtyp;
        header.data["type"] = rosname;
        header.data["md5sum"] = (md5sum!="")?md5sum:"*";
        if (message_definition!="") {
            header.data["message_definition"] = message_definition;
        }
    }
    dbg_printf("<outgoing> Type of data is %s\n", rosname.c_str());

    if (header.data.find("callerid")!=header.data.end()) {
        proto.setRoute(proto.getRoute().addFromName(header.data["callerid"].c_str()));
    } else {
        proto.setRoute(proto.getRoute().addFromName("tcpros"));
    }

    // Let's just ignore everything that is sane and holy, and
    // send the same header right back.
    // **UPDATE** Oh, ok, let's modify the callerid.  Begrudgingly.
    NestedContact nc(proto.getRoute().getToName());
    header.data["callerid"] = nc.getNodeName().c_str();

    string header_serial = header.writeHeader();
    string header_len(4,'\0');
    char *at = (char*)header_len.c_str();
    RosHeader::appendInt(at,header_serial.length());
    dbg_printf("Writing %s -- %d bytes\n", 
               RosHeader::showMessage(header_len).c_str(),
               (int)header_len.length());
    
    Bytes b1((char*)header_len.c_str(),header_len.length());
    proto.os().write(b1);
    dbg_printf("Writing %s -- %d bytes\n", 
               RosHeader::showMessage(header_serial).c_str(),
               (int)header_serial.length());
    Bytes b2((char*)header_serial.c_str(),header_serial.length());
    proto.os().write(b2);

    if (header.data.find("probe")!=header.data.end()) {
        dbg_printf("================PROBE===============\n");
        return false;
    }


    if (!isService) {
        isService = (header.data.find("service")!=header.data.end());
    }
    if (rosname!="" && (user_type != wire_type || user_type == "")) {
        if (wire_type!="sensor_msgs/Image") { // currently using a custom method for images
            kind = TcpRosStream::rosToKind(rosname.c_str()).c_str();
            TcpRosStream::configureTwiddler(twiddler,kind.c_str(),rosname.c_str(),true,true);
            translate = TCPROS_TRANSLATE_TWIDDLER;
        }
    } else {
        rosname = "";
    }
    sender = isService; 

    processRosHeader(header);

    if (isService) {
        TcpRosStream *stream = new TcpRosStream(proto.giveStreams(),sender,
                                                false,
                                                isService,raw,rosname.c_str());
        
        if (stream==NULL) { return false; }
        proto.takeStreams(stream);
        return proto.is().isOk();
    }

    return true;
}