[TeamTalk 306]: [842] trunk/TeamTalk/Libraries/boeingLib: Import commits from darcs repo, including location error messaging.
tk@edam.speech.cs.cmu.edu
tk at edam.speech.cs.cmu.edu
Mon Oct 15 13:12:09 EDT 2007
An HTML attachment was scrubbed...
URL: http://mailman.srv.cs.cmu.edu/pipermail/teamtalk-developers/attachments/20071015/a04d4042/attachment.html
-------------- next part --------------
Modified: trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_map_server.cc
===================================================================
--- trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_map_server.cc 2007-10-14 03:51:54 UTC (rev 841)
+++ trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_map_server.cc 2007-10-15 17:12:08 UTC (rev 842)
@@ -4,6 +4,10 @@
#include <stdarg.h>
#include <stdio.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+
#include "boeing_map_server.h"
#include "timer.h"
#include "udpsocket.h"
@@ -12,7 +16,6 @@
static const bool debug_flag = false;
-
//=== Debugging ================================
static void debug(char const *fmt,...)
{
@@ -59,19 +62,28 @@
//=== Recieving ========================================
-const MsgMapServer* MapServer::getNextMessage()
+const MsgMapServer* MapServer::getNextMessage(struct sockaddr_in* addr)
{
+
+ //fprintf(stderr,"%s 1\n",__FUNCTION__);
+ //fprintf(stderr,"%s:2 0x%x \n",__FUNCTION__,addr);
+
if (!isConnected()) {
printf("WARNING: mapserver Not connected\n");
return NULL;
}
+ //fprintf(stderr,"%s:3 0x%x \n",__FUNCTION__,addr);
+
// is there anything to get?
- if (!sock->isConnected() || !sock->readyForRecv())
+ // if (!sock->isConnected() || !sock->readyForRecv())
+ if (!sock->isConnected())
return (NULL);
+ // fprintf(stderr,"%s:4 0x%x \n",__FUNCTION__,addr);
+
// receive the header...
- int rv = sock->recv(rxdata.buff,sizeof(rxdata));
+ int rv = sock->recv(rxdata.buff,sizeof(rxdata),addr);
if (rv>0 && rv!=rxdata.hdr.len) {
printf("WARNING: size mismatch! %i, but packet says %i\n",
@@ -85,33 +97,51 @@
//=== Sending ================================
// raw send methods...use wisely
-bool MapServer::doSend(MsgHeader *msg)
+bool MapServer::doSend(MsgHeader *msg, struct sockaddr_in* addr)
{
- if (!isConnected() && !sock->readyForSend())
+ int ret;
+
+ //fprintf(stderr,"%s:1 0x%x \n",__FUNCTION__,addr);
+
+ if (!isConnected() && !sock->readyForSend()){
+ fprintf(stderr,"%s:ERROR\n",__FUNCTION__);
return (false);
-
+ }
+
+ //fprintf(stderr,"%s:2 0x%x \n",__FUNCTION__,addr);
+
msg->tstamp = Profiler::getTimeOfDay();
debug("DEBUG: sending msg hdr (t %x len %i t %f)\n",
msg->type, msg->len, msg->tstamp);
- return (sock->send(msg, msg->len)!=0);
+ if(NULL == addr){
+ //fprintf(stderr,"%s:addr == NULL\n",__FUNCTION__);
+ ret = sock->send(msg, msg->len)!=0;
+ }else{
+ //fprintf(stderr,"%s: %d %d\n",__FUNCTION__,addr->sin_addr.s_addr,addr->sin_port);
+ ret = sock->SendTo(addr,msg, msg->len)!=0;
+ }
+
+ return ret;
}
// sending operations
-bool MapServer::sendFullMap(MsgMap *msg)
+bool MapServer::sendFullMap(MsgMap *msg,sockaddr_in *addr)
{
msg->len = msg->getSize();
msg->type = MAP_FULL;
- return (doSend((MsgHeader *) msg));
+ return (doSend((MsgHeader *) msg,addr));
}
-bool MapServer::sendDiffMap(MsgMap *msg)
+bool MapServer::sendDiffMap(MsgMap *msg,struct sockaddr_in *addr)
{
+ //fprintf(stderr,"%s:1 %d %d\n",__FUNCTION__,addr->sin_addr.s_addr,addr->sin_port);
msg->len = msg->getSize();
+ //fprintf(stderr,"%s:2 0x%x \n",__FUNCTION__,addr);
msg->type = MAP_DIFF;
- return (doSend((MsgHeader *) msg));
+ return (doSend((MsgHeader *) msg,addr));
}
int MapServer::getFD() {
Modified: trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_map_server.h
===================================================================
--- trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_map_server.h 2007-10-14 03:51:54 UTC (rev 841)
+++ trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_map_server.h 2007-10-15 17:12:08 UTC (rev 842)
@@ -5,6 +5,8 @@
#ifndef __BOEING_MAP_SERVER_H__
#define __BOEING_MAP_SERVER_H__
+#include <netinet/in.h>
+
#include "boeing_map_packet.h"
class UDPSocket;
@@ -23,13 +25,13 @@
//=== Rx Methods ======================================
/// get the next message (if there is one). Returns NULL if there is none
- const MsgMapServer* getNextMessage();
+ const MsgMapServer* getNextMessage(struct sockaddr_in *addr=NULL);
//=== Send Methods ====================================
// sending operations
- bool sendFullMap(MsgMap *msg);
- bool sendDiffMap(MsgMap *msg);
+ bool sendFullMap(MsgMap *msg,struct sockaddr_in *addr=NULL);
+ bool sendDiffMap(MsgMap *msg,struct sockaddr_in *addr=NULL);
//=== Accessors ========================================
int getFD();
@@ -37,7 +39,7 @@
private:
// raw send methods...use wisely
- bool doSend(MsgHeader *msg);
+ bool doSend(MsgHeader *msg,struct sockaddr_in *addr=NULL);
private:
// socket controls
Modified: trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_robot_client.cc
===================================================================
--- trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_robot_client.cc 2007-10-14 03:51:54 UTC (rev 841)
+++ trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_robot_client.cc 2007-10-15 17:12:08 UTC (rev 842)
@@ -2,7 +2,6 @@
Boeing robot client ==
*/
-#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
Modified: trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_robot_packet.h
===================================================================
--- trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_robot_packet.h 2007-10-14 03:51:54 UTC (rev 841)
+++ trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_robot_packet.h 2007-10-15 17:12:08 UTC (rev 842)
@@ -45,6 +45,10 @@
ROB_IMAGE,ROB_PLAY_HALT,
ROB_EVAL
};
+
+ enum RobotErrorID {
+ ROB_OK=0, ROB_POS_ERROR=1,ROB_LASER_ERROR=1,ROB_NO_ARCS=3
+ };
static const uint16_t CmdMsgIdMask = 0x00FF;
static const uint16_t RobMsgIdMask = 0xFF00;
@@ -269,9 +273,10 @@
float x, y;
float angle;
int16_t moving;
- static MsgRobLocation factory(float x, float y, float angle, int16_t moving) {
+ int16_t error;
+ static MsgRobLocation factory(float x, float y, float angle, int16_t moving, int16_t error) {
MsgRobLocation m;
- m.x = x; m.y = y; m.angle = angle; m.moving = moving;
+ m.x = x; m.y = y; m.angle = angle; m.moving = moving; m.error=error;
return m;
}
bool operator!=(const MsgRobLocation& rhs) {
Modified: trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_robot_server.cc
===================================================================
--- trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_robot_server.cc 2007-10-14 03:51:54 UTC (rev 841)
+++ trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_robot_server.cc 2007-10-15 17:12:08 UTC (rev 842)
@@ -294,7 +294,7 @@
return (doSend((MsgHeader *) &msg));
}
- bool RobotServer::sendLocation(float x, float y, float a, bool moving)
+ bool RobotServer::sendLocation(float x, float y, float a, bool moving, int error)
{
MsgRobLocation msg;
@@ -303,6 +303,7 @@
msg.y = y;
msg.angle = a;
msg.moving = moving;
+ msg.error = error;
msg.len = sizeof(msg);
msg.type = ROB_LOCATION;
Modified: trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_robot_server.h
===================================================================
--- trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_robot_server.h 2007-10-14 03:51:54 UTC (rev 841)
+++ trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_robot_server.h 2007-10-15 17:12:08 UTC (rev 842)
@@ -64,7 +64,7 @@
bool sendAborted(int taskid);
/// send the updated robot pose
- bool sendLocation(float x, float y, float a, bool moving);
+ bool sendLocation(float x, float y, float a, bool moving, int error=0);
/** Send a compressed image to the server
the image is a pointer to a jpeg image file
Modified: trunk/TeamTalk/Libraries/boeingLib/coralshared/memfile.h
===================================================================
--- trunk/TeamTalk/Libraries/boeingLib/coralshared/memfile.h 2007-10-14 03:51:54 UTC (rev 841)
+++ trunk/TeamTalk/Libraries/boeingLib/coralshared/memfile.h 2007-10-15 17:12:08 UTC (rev 842)
@@ -16,7 +16,7 @@
off_t len;
int fd;
public:
- memfile() {buf=NULL; len=0;}
+ memfile() {buf=NULL; len=0; fd=0;}
~memfile() {mclose();}
void *mopen(const char *filename,off_t &length);
Modified: trunk/TeamTalk/Libraries/boeingLib/coralshared/udpsocket.cc
===================================================================
--- trunk/TeamTalk/Libraries/boeingLib/coralshared/udpsocket.cc 2007-10-14 03:51:54 UTC (rev 841)
+++ trunk/TeamTalk/Libraries/boeingLib/coralshared/udpsocket.cc 2007-10-15 17:12:08 UTC (rev 842)
@@ -9,6 +9,7 @@
/* LICENSE: */
+#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
@@ -203,8 +204,11 @@
return status;
}
-int UDPSocket::recv(void* buf, int maxbytes)
+int UDPSocket::recv(void* buf, int maxbytes,struct sockaddr_in* addr)
{
+
+ //fprintf(stderr,"%s: addr 0x%x\n",__FUNCTION__,addr);
+
if (status <= 0)
return 0;
@@ -228,11 +232,10 @@
ack();
} else {
-
// SERVER SIDE UDP
// For UDP just read from the fd.
while(last_message_bytes == 0){
- udpServerRecv();
+ udpServerRecv(addr);
}
if (maxbytes > last_message_bytes)
@@ -246,7 +249,7 @@
return rv;
}
-void UDPSocket::udpServerRecv()
+void UDPSocket::udpServerRecv(struct sockaddr_in* addr)
{
struct sockaddr_in from;
socklen_t fromlen = sizeof(from);
@@ -254,7 +257,18 @@
last_message_bytes = recvfrom(fd, last_message, MaxMessageSize, 0,
(sockaddr *) &from, &fromlen);
+ //fprintf(stderr,"%s: addr 0x%x\n",__FUNCTION__,addr);
+
+ if(addr != NULL){
+ //fprintf(stderr,"Adder is Not NULL\n");
+ memcpy(addr,&from,sizeof(struct sockaddr_in));
+ }else{
+ //fprintf(stderr,"Adder IS NULL\n");
+ }
+
acked(from);
+
+
}
int UDPSocket::send(const void* buf, int nbytes)
@@ -317,9 +331,9 @@
// SERVER SIDE UDP
// Write to the address of each client.
- rv = ::sendto(fd, buf, nbytes, 0,
+ rv = MIN(rv,::sendto(fd, buf, nbytes, 0,
(sockaddr *) &(udp_clients[cli].a),
- sizeof(udp_clients[cli].a));
+ sizeof(udp_clients[cli].a)));
double tack = udp_clients[cli].lastacktime + ack_timeout;
@@ -333,13 +347,22 @@
rv = 0;
}
- if (rv < 0)
+ if (rv < 0)
perror("send (server, UDP)");
- return rv;
+ return 0;
}
+int UDPSocket::SendTo(struct sockaddr_in* addr,const void* buf, int nbytes){
+
+ for(unsigned int i=0; i<udp_clients.size(); i++) {
+ if (memcmp(&udp_clients[i].a, addr, sizeof(struct sockaddr_in)) == 0) {
+ return send(i,buf,nbytes);
+ }
+ }
+ return 0;
+}
void UDPSocket::accept()
{
@@ -413,7 +436,7 @@
pfd.fd = fd;
pfd.events = POLLIN;
- if (poll(&pfd, 1, timeout*1e3) <= 0)
+ if (poll(&pfd, 1, (int)ceil(timeout*1e3)) <= 0)
return(false);
// if (pfd.revents & (POLLERR | POLLNVAL)) {
Modified: trunk/TeamTalk/Libraries/boeingLib/coralshared/udpsocket.h
===================================================================
--- trunk/TeamTalk/Libraries/boeingLib/coralshared/udpsocket.h 2007-10-14 03:51:54 UTC (rev 841)
+++ trunk/TeamTalk/Libraries/boeingLib/coralshared/udpsocket.h 2007-10-15 17:12:08 UTC (rev 842)
@@ -12,6 +12,7 @@
#ifndef __UDPSOCKET_H__
#define __UDPSOCKET_H__
+#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <fcntl.h>
@@ -102,7 +103,7 @@
char *last_message;
int last_message_bytes;
- void udpServerRecv();
+ void udpServerRecv(struct sockaddr_in* addr=NULL);
double getTime();
@@ -129,9 +130,10 @@
int isConnected() const { return status>0; }
// Returns the number of bytes read or written.
- int recv(void *buf, int maxbytes);
+ int recv(void *buf, int maxbytes, struct sockaddr_in *addr = NULL);
int send(const void *buf, int nbytes);
int send(int cli,const void *buf, int nbytes);
+ int SendTo(struct sockaddr_in *addr,const void *buf, int nbytes);
int recvType(void *buf, int maxbytes, char &type) {
int rv = recv(buf, maxbytes); type = ((char *) buf)[0]; return rv; }
More information about the TeamTalk-developers
mailing list