void send_mem( TCP_NODE *n ) {
ssize_t bytes_sent = 0;
int bytes_todo = 0;
char *p = NULL;
while( status == RUMBLE ) {
p = n->send_buf + n->send_offset;
bytes_todo = n->send_size - n->send_offset;
bytes_sent = send( n->connfd, p, bytes_todo, 0 );
if( bytes_sent < 0 ) {
if( errno == EAGAIN || errno == EWOULDBLOCK ) {
return;
}
/* Client closed the connection, etc... */
node_status( n, NODE_MODE_SHUTDOWN );
return;
}
n->send_offset += bytes_sent;
/* Done */
if( n->send_offset >= n->send_size ) {
node_status( n, NODE_MODE_SEND_FILE );
return;
}
}
}
static void find_all_domain_master_names_query_success(struct subnet_record *subrec,
struct userdata_struct *userdata_in,
struct nmb_name *q_name, struct in_addr answer_ip, struct res_rec *rrec)
{
/*
* We now have a list of all the domain master browsers for all workgroups
* that have registered with the WINS server. Now do a node status request
* to each one and look for the first 1b name in the reply. This will be
* the workgroup name that we will add to the unicast subnet as a 'non-local'
* workgroup.
*/
struct nmb_name nmbname;
struct in_addr send_ip;
int i;
if( DEBUGLVL( 5 ) )
{
dbgtext( "find_all_domain_master_names_query_succes:\n" );
dbgtext( "Got answer from WINS server of %d ", (rrec->rdlength / 6) );
dbgtext( "IP addresses for Domain Master Browsers.\n" );
}
for(i = 0; i < rrec->rdlength / 6; i++)
{
/* Initiate the node status requests. */
make_nmb_name(&nmbname, "*", 0);
putip((char *)&send_ip, (char *)&rrec->rdata[(i*6) + 2]);
/*
* Don't send node status requests to ourself.
*/
if(ismyip( send_ip ))
{
if( DEBUGLVL( 5 ) )
{
dbgtext( "find_all_domain_master_names_query_succes:\n" );
dbgtext( "Not sending node status to our own IP " );
dbgtext( "%s.\n", inet_ntoa(send_ip) );
}
continue;
}
if( DEBUGLVL( 5 ) )
{
dbgtext( "find_all_domain_master_names_query_success:\n" );
dbgtext( "Sending node status request to IP %s.\n", inet_ntoa(send_ip) );
}
node_status( subrec, &nmbname, send_ip,
get_domain_master_name_node_status_success,
get_domain_master_name_node_status_fail,
NULL);
}
}
void send_cork_start( TCP_NODE *n ) {
int on = 1;
if( setsockopt( n->connfd, IPPROTO_TCP, TCP_CORK, &on, sizeof(on)) != 0 ) {
fail( strerror( errno ) );
}
node_status( n, NODE_MODE_SEND_MEM );
}
void http_send(TCP_NODE * n)
{
if (n->pipeline == NODE_SHUTDOWN) {
return;
}
/* Prepare event system for sending data */
node_status(n, NODE_SEND_INIT);
/* Start sending */
send_tcp(n);
}
void tcp_input( ITEM *listItem ) {
TCP_NODE *n = list_value( listItem );
char buffer[BUF_SIZE];
ssize_t bytes = 0;
while( status == RUMBLE ) {
/* Reset timeout counter */
node_activity( n );
/* Get data */
bytes = recv( n->connfd, buffer, BUF_OFF1, 0 );
if( bytes < 0 ) {
if( errno == EAGAIN || errno == EWOULDBLOCK ) {
return;
} else if( errno == ECONNRESET ) {
/*
* Very common behaviour
* info( &n->c_addr, 0, "Connection reset by peer" );
*/
node_status( n, NODE_MODE_SHUTDOWN );
return;
} else {
info( &n->c_addr, 0, "recv() failed:" );
info( &n->c_addr, 0, strerror( errno ) );
return;
}
} else if( bytes == 0 ) {
/* Regular shutdown */
node_status( n, NODE_MODE_SHUTDOWN );
return;
} else {
/* Read */
tcp_buffer( n, buffer, bytes );
return;
}
}
}
void tcp_buffer( TCP_NODE *n, char *buffer, ssize_t bytes ) {
/* Append buffer */
node_appendBuffer( n, buffer, bytes );
if( n->mode != NODE_MODE_READY ) {
fail( "FIXME tcp_buffer..." );
}
/* Overflow? */
if( n->recv_size >= BUF_OFF1 ) {
info( &n->c_addr, 500, "Max head buffer exceeded..." );
node_status( n, NODE_MODE_SHUTDOWN );
return;
}
http_buf( n );
}
static void traverse_subtree( struct ip_node *node, int depth, int options )
{
static unsigned char ip_addr[MAX_DEPTH];
struct ip_node *foo;
DBG("pike:rpc traverse_subtree, depth: %d, byte: %d", depth, node->byte);
assert( depth < MAX_DEPTH );
ip_addr[depth] = node->byte;
if ( node->flags & NODE_IPLEAF_FLAG ) {
int ns = node_status(node);
DBG("pike:traverse_subtree: options: 0x%02x, node status: 0x%02x", options, ns);
/* add to the result list if it has requested status */
switch (options) {
case NODE_STATUS_HOT:
if ( ns & NODE_STATUS_HOT )
pike_top_add_entry(ip_addr, depth+1, node->leaf_hits, node->hits, node->expires - get_ticks(), ns);
break;
case NODE_STATUS_ALL:
pike_top_add_entry(ip_addr, depth+1, node->leaf_hits, node->hits, node->expires - get_ticks(), ns);
break;
}
}
else if (! node->kids) { /* TODO non IP leaf of ip_tree - it is possible to report WARM nodes here */
/* if ( options == ns )
pike_top_add_entry(ip_addr, depth+1, node->leaf_hits, node->hits, node->expires - get_ticks(), ns);
*/ }
else { /* not a any kind of leaf - inner node */
DBG("pike:rpc traverse_subtree, not IP leaf, depth: %d, ip: %d.%d.%d.%d hits[%d,%d], expires: %d",
depth, ip_addr[0], ip_addr[1], ip_addr[2], ip_addr[3], node->hits[0], node->hits[1], node->expires - get_ticks());
}
foo = node->kids;
while (foo) {
traverse_subtree( foo, depth + 1, options );
foo = foo->next;
}
}
void tcp_buffer( TCP_NODE *n, char *buffer, ssize_t bytes ) {
/* Append buffer */
node_appendBuffer( n, buffer, bytes );
if( n->pipeline != NODE_READY ) {
fail( "FIXME tcp_buffer..." );
}
/* Overflow? */
if( n->recv_size >= BUF_OFF1 ) {
info( &n->c_addr, "Max head buffer exceeded..." );
node_status( n, NODE_SHUTDOWN );
return;
}
/* Parse request */
http_buf( n );
/* Start sending data */
http_send( n );
}
void send_cork_stop( TCP_NODE *n ) {
int off = 0;
if( n->mode != NODE_MODE_SEND_STOP ) {
return;
}
if( setsockopt( n->connfd, IPPROTO_TCP, TCP_CORK, &off, sizeof(off)) != 0 ) {
fail( strerror( errno ) );
}
/* Clear input and output buffers */
node_clearSendBuf( n );
/* Mark TCP server ready */
node_status( n, NODE_MODE_READY );
/* HTTP Pipeline: There is at least one more request to parse. */
if( n->recv_size > 0 ) {
http_buf( n );
}
}
static void find_domain_master_name_query_success(struct subnet_record *subrec,
struct userdata_struct *userdata_in,
struct nmb_name *q_name, struct in_addr answer_ip, struct res_rec *rrec)
{
/*
* Unfortunately, finding the IP address of the Domain Master Browser,
* as we have here, is not enough. We need to now do a sync to the
* SERVERNAME<0x20> NetBIOS name, as only recent NT servers will
* respond to the SMBSERVER name. To get this name from IP
* address we do a Node status request, and look for the first
* NAME<0x20> in the response, and take that as the server name.
* We also keep a cache of the Domain Master Browser name for this
* workgroup in the Workgroup struct, so that if the same IP addess
* is returned every time, we don't need to do the node status
* request.
*/
struct work_record *work;
struct nmb_name nmbname;
struct userdata_struct *userdata;
int size = sizeof(struct userdata_struct) + sizeof(fstring)+1;
if( !(work = find_workgroup_on_subnet(subrec, q_name->name)) )
{
if( DEBUGLVL( 0 ) )
{
dbgtext( "find_domain_master_name_query_success:\n" );
dbgtext( "Failed to find workgroup %s\n", q_name->name );
}
return;
}
/* First check if we already have a dmb for this workgroup. */
if(!is_zero_ip(work->dmb_addr) && ip_equal(work->dmb_addr, answer_ip))
{
/* Do the local master browser announcement to the domain
master browser name and IP. */
announce_local_master_browser_to_domain_master_browser( work );
/* Now synchronise lists with the domain master browser. */
sync_with_dmb(work);
return;
}
else
zero_ip(&work->dmb_addr);
/* Now initiate the node status request. */
make_nmb_name(&nmbname,"*",0x0);
/* Put the workgroup name into the userdata so we know
what workgroup we're talking to when the reply comes
back. */
/* Setup the userdata_struct - this is copied so we can use
a stack variable for this. */
if((userdata = (struct userdata_struct *)malloc(size)) == NULL)
{
DEBUG(0, ("find_domain_master_name_query_success: malloc fail.\n"));
return;
}
userdata->copy_fn = NULL;
userdata->free_fn = NULL;
userdata->userdata_len = strlen(work->work_group)+1;
pstrcpy(userdata->data, work->work_group);
node_status( subrec, &nmbname, answer_ip,
domain_master_node_status_success,
domain_master_node_status_fail,
userdata);
zero_free(userdata, size);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "publish_objects_tf_frames");
ros::NodeHandle n;
tf::TransformBroadcaster br;
std::vector<moveit_msgs::CollisionObject> cos;
std::vector<moveit_msgs::AttachedCollisionObject> acos;
// boost::this_thread::sleep(boost::posix_time::seconds(3));
// geometry_msgs::PoseStamped cam_pose = getCamPose(n);
geometry_msgs::PoseStamped temp_pose;
ROS_INFO_STREAM("tf publisher started..........");
ros::service::waitForService(move_group::GET_PLANNING_SCENE_SERVICE_NAME);
ros::ServiceClient ps_service_client = n.serviceClient<moveit_msgs::GetPlanningScene>(move_group::GET_PLANNING_SCENE_SERVICE_NAME);
ros::WallDuration(1.0).sleep();
suturo_manipulation::NodeStatus node_status(n);
node_status.nodeStarted(suturo_manipulation_msgs::ManipulationNodeStatus::NODE_PUBLISH_OBJECT_FRAMES);
while (getObjects(ps_service_client, cos) && getAttachedObjects(ps_service_client, acos))
{
//publish tf frame in every collisionobject
// if ()
// {
// ROS_INFO_STREAM(cos.size());
for (std::vector<moveit_msgs::CollisionObject>::iterator co = cos.begin(); co != cos.end(); ++co)
{
if (co->primitive_poses.size() > 0)
{
temp_pose.pose = co->primitive_poses[0];
temp_pose.header = co->header;
publishTfFrame(co->id, temp_pose, br);
}
}
// }
// if (getAttachedObjects(ps_service_client, acos))
// {
for (std::vector<moveit_msgs::AttachedCollisionObject>::iterator co = acos.begin(); co != acos.end(); ++co)
{
if (co->object.primitive_poses.size() > 0)
{
temp_pose.pose = co->object.primitive_poses[0];
temp_pose.header = co->object.header;
publishTfFrame(co->object.id, temp_pose, br);
}
}
// }
//publish tf frame in every attachedobject
// for (std::vector<moveit_msgs::AttachedCollisionObject>::iterator aco = acos.begin(); aco != acos.end(); ++aco)
// {
// if (aco->object.primitive_poses.size() >= 1)
// {
// ROS_DEBUG_STREAM(*aco);
// temp_pose.pose = aco->object.primitive_poses[0];
// temp_pose.header = aco->object.header;
// publishTfFrame(aco->object.id, temp_pose, transform, br);
// }
// else
// {
// ROS_DEBUG_STREAM(*aco);
// temp_pose.pose = aco->object.mesh_poses[0];
// temp_pose.header = aco->object.header;
// publishTfFrame(aco->object.id, temp_pose, transform, br);
// }
// }
//publish cam_frame
// publishTfFrame("webcam", cam_pose, transform, br);
ros::WallDuration(0.1).sleep();
}
return 0;
}
void http_read(TCP_NODE * n, char *p_cmd, char *p_url, char *p_proto,
HASH * p_head)
{
int code = 0;
char lastmodified[DATE_SIZE];
char resource[BUF_SIZE];
char filename[BUF_SIZE];
char range[BUF_SIZE];
char *p_range = range;
size_t filesize = 0;
size_t content_length = 0;
char keepalive[BUF_SIZE];
const char *mimetype = NULL;
/* Get protocol */
if (!http_proto(n, p_proto)) {
node_status(n, NODE_SHUTDOWN);
goto END;
}
/* Get action */
if (!http_action(n, p_cmd)) {
node_status(n, NODE_SHUTDOWN);
goto END;
}
/* Get resource */
if (!http_resource(n, p_url, resource)) {
node_status(n, NODE_SHUTDOWN);
goto END;
}
/* Check Keep-Alive */
n->keepalive = http_keepalive(p_head, keepalive);
/* Compute filename */
if (!http_filename(resource, filename)) {
http_404(n, keepalive);
info(_log, &n->c_addr, "404 %s", resource);
goto END;
}
/* Compute file size */
filesize = http_size_simple(filename);
/* Compute mime type */
mimetype = mime_find(filename);
/* Last-Modified. */
if (!http_resource_modified(filename, p_head, lastmodified)) {
http_304(n, keepalive);
info(_log, &n->c_addr, "304 %s", resource);
goto END;
}
/* Range request? */
if (http_range_detected(p_head, range)) {
code = 206;
} else {
code = 200;
}
/* Normal 200-er request */
if (code == 200) {
info(_log, &n->c_addr, "200 %s", resource);
http_200(n, lastmodified, filename, filesize, keepalive,
mimetype);
http_body(n, filename, filesize);
goto END;
}
/* Check for 'bytes=' in range. Fallback to 200 if necessary. */
if (!http_range_prepare(&p_range)) {
info(_log, &n->c_addr, "200 %s", resource);
http_200(n, lastmodified, filename, filesize, keepalive,
mimetype);
http_body(n, filename, filesize);
goto END;
}
/* multipart/byteranges */
if (!http_range_multipart(p_range)) {
RESPONSE *r_head = NULL, *r_file = NULL;
/* Header */
if ((r_head =
resp_put(n->response, RESPONSE_FROM_MEMORY)) == NULL) {
node_status(n, NODE_SHUTDOWN);
goto END;
}
/* File */
if ((r_file =
resp_put(n->response, RESPONSE_FROM_FILE)) == NULL) {
node_status(n, NODE_SHUTDOWN);
goto END;
}
/* Parse range. */
if (!http_range_simple(n, r_file, filename, filesize, p_range,
&content_length)) {
node_status(n, NODE_SHUTDOWN);
goto END;
}
/* Header with known content_length */
http_206_simple(n, r_head, r_file, lastmodified, filename,
filesize, content_length, keepalive, mimetype);
info(_log, &n->c_addr, "206 %s [%s]", resource, range);
goto END;
} else {
RESPONSE *r_head = NULL, *r_bottom = NULL, *r_zsyncbug = NULL;
char boundary[12];
/* Create boundary string */
http_random(boundary, 12);
/* Header */
if ((r_head =
resp_put(n->response, RESPONSE_FROM_MEMORY)) == NULL) {
node_status(n, NODE_SHUTDOWN);
goto END;
}
/* zsync bug? One more \r\n between header and body. */
if ((r_zsyncbug =
resp_put(n->response, RESPONSE_FROM_MEMORY)) == NULL) {
node_status(n, NODE_SHUTDOWN);
goto END;
}
/* Parse range. */
if (!http_range_complex
(n, filename, filesize, mimetype, p_range, &content_length,
boundary)) {
node_status(n, NODE_SHUTDOWN);
goto END;
}
/* Bottom */
if ((r_bottom =
resp_put(n->response, RESPONSE_FROM_MEMORY)) == NULL) {
node_status(n, NODE_SHUTDOWN);
goto END;
}
http_206_boundary_finish(r_bottom, &content_length, boundary);
/* Header with known content_length */
http_206_complex(n, r_head, lastmodified, filename,
filesize, content_length, boundary, keepalive);
/* zsync bug? One more \r\n between header and body. */
http_newline(r_zsyncbug, &content_length);
info(_log, &n->c_addr, "206 %s [%s]", resource, range);
goto END;
}
info(_log, &n->c_addr, "FIXME: HTTP parser end reached without result");
node_status(n, NODE_SHUTDOWN);
END:
/* HTTP Pipeline: There is at least one more request to parse.
* Recursive request! Limited by the input buffer.
*/
if (n->pipeline != NODE_SHUTDOWN && n->recv_size > 0) {
http_buf(n);
}
}
void http_buf(TCP_NODE * n)
{
char *p_cmd = NULL;
char *p_url = NULL;
char *p_var = NULL;
char *p_proto = NULL;
char *p_head = NULL;
char *p_body = NULL;
HASH *head = NULL;
char recv_buf[BUF_SIZE];
/* Do not start before at least this much arrived: "GET / HTTP/1.1" */
if (n->recv_size <= 14) {
return;
}
/* Copy HTTP request */
memset(recv_buf, '\0', BUF_SIZE);
memcpy(recv_buf, n->recv_buf, n->recv_size);
/* Return until the header is complete. Gets killed if this grows beyond
* all imaginations. */
if ((p_body = strstr(recv_buf, "\r\n\r\n")) == NULL) {
return;
}
/* Keep one \r\n for consistent header analysis. */
*p_body = '\r';
p_body++;
*p_body = '\n';
p_body++;
*p_body = '\0';
p_body++;
*p_body = '\0';
p_body++;
/* HTTP Pipelining: There is at least one more request. */
if (n->recv_size > (ssize_t) (p_body - recv_buf)) {
n->recv_size -= (ssize_t) (p_body - recv_buf);
memset(n->recv_buf, '\0', BUF_SIZE);
memcpy(n->recv_buf, p_body, n->recv_size);
} else {
/* The request has been copied. Reset the receive buffer. */
node_clearRecvBuf(n);
}
/* Remember start point */
p_cmd = recv_buf;
/* Find url */
if ((p_url = strchr(p_cmd, ' ')) == NULL) {
info(_log, &n->c_addr, "Requested URL was not found");
node_status(n, NODE_SHUTDOWN);
return;
}
*p_url = '\0';
p_url++;
/* Find protocol type */
if ((p_proto = strchr(p_url, ' ')) == NULL) {
info(_log, &n->c_addr, "No protocol found in request");
node_status(n, NODE_SHUTDOWN);
return;
}
*p_proto = '\0';
p_proto++;
/* Find variables( Start from p_url again) */
if ((p_var = strchr(p_url, '?')) != NULL) {
*p_var = '\0';
p_var++;
}
/* Find header lines */
if ((p_head = strstr(p_proto, "\r\n")) == NULL) {
info(_log, &n->c_addr,
"There must be a \\r\\n. I put it there...");
node_status(n, NODE_SHUTDOWN);
return;
}
*p_head = '\0';
p_head++;
*p_head = '\0';
p_head++;
/* Hash header */
head = http_hashHeader(p_head);
/* Validate input */
http_read(n, p_cmd, p_url, p_proto, head);
/* Delete Hash */
http_deleteHeader(head);
}
void send_file( TCP_NODE *n ) {
ssize_t bytes_sent = 0;
int fh = 0;
if( n->mode != NODE_MODE_SEND_FILE ) {
return;
}
/* Nothing to do */
if( n->filesize == 0 ) {
node_status( n, NODE_MODE_SEND_STOP );
return;
}
/* Not modified. Stop here. */
if( n->code == 304 ) {
node_status( n, NODE_MODE_SEND_STOP );
return;
}
/* Not found */
if( n->code == 404 ) {
node_status( n, NODE_MODE_SEND_STOP );
return;
}
/* HEAD request. Stop here. */
if( n->type == HTTP_HEAD ) {
node_status( n, NODE_MODE_SEND_STOP );
return;
}
while( status == RUMBLE ) {
fh = open( n->filename, O_RDONLY );
if( fh < 0 ) {
info( NULL, 500, "Failed to open %s", n->filename );
fail( strerror( errno) );
}
/* The SIGPIPE gets catched in sig.c */
bytes_sent = sendfile( n->connfd, fh, &n->f_offset, n->content_length );
if( close( fh) != 0 ) {
info( NULL, 500, "Failed to close %s", n->filename );
fail( strerror( errno) );
}
if( bytes_sent < 0 ) {
if( errno == EAGAIN || errno == EWOULDBLOCK ) {
/* connfd is blocking */
return;
}
/* Client closed the connection, etc... */
node_status( n, NODE_MODE_SHUTDOWN );
return;
}
/* Done */
if( n->f_offset >= n->f_stop ) {
/* Clean up */
node_status( n, NODE_MODE_SEND_STOP );
return;
}
}
}
