From 5defa7a96290284beb11809413d49974af331fbf Mon Sep 17 00:00:00 2001 From: Vivek Date: Thu, 15 Sep 2016 17:59:09 -0400 Subject: [PATCH] Initial commit after adding dummy communication --- rosbuzz/src/buzz_utility.c | 97 ++++++++++++++++--------------- rosbuzz/src/rosbuzz.cpp | 113 ++++++++++++++++--------------------- 2 files changed, 99 insertions(+), 111 deletions(-) diff --git a/rosbuzz/src/buzz_utility.c b/rosbuzz/src/buzz_utility.c index 64266be..98e3eab 100644 --- a/rosbuzz/src/buzz_utility.c +++ b/rosbuzz/src/buzz_utility.c @@ -394,6 +394,8 @@ void check_swarm_members(const void* key, void* data, void* params) { } void buzz_script_step() { + + /* * Process incoming messages */ @@ -401,55 +403,55 @@ void buzz_script_step() { buzzneighbors_reset(VM); /* Lock mutex */ /* fprintf(stderr, "[DEBUG] Processing incoming packets...\n"); */ - pthread_mutex_lock(&INCOMING_PACKET_MUTEX); + //pthread_mutex_lock(&INCOMING_PACKET_MUTEX); /* Go through messages and add them to the FIFO */ - struct incoming_packet_s* n; - while(PACKETS_FIRST) { + //struct incoming_packet_s* n; + //while(PACKETS_FIRST) { /* Save next packet */ - n = PACKETS_FIRST->next; + // n = PACKETS_FIRST->next; /* Update Buzz neighbors information */ - buzzneighbors_add(VM, PACKETS_FIRST->id, 0.0, 0.0, 0.0); + // buzzneighbors_add(VM, PACKETS_FIRST->id, 0.0, 0.0, 0.0); /* Go through the payload and extract the messages */ - uint8_t* pl = PACKETS_FIRST->payload; - size_t tot = 0; + // uint8_t* pl = PACKETS_FIRST->payload; + //size_t tot = 0; uint16_t msgsz; /* fprintf(stderr, "[DEBUG] Processing packet %p from %d\n", */ /* PACKETS_FIRST, */ /* PACKETS_FIRST->id); */ /* fprintf(stderr, "[DEBUG] recv sz = %u\n", */ /* *(uint16_t*)pl); */ - do { + //do { /* Get payload size */ - msgsz = *(uint16_t*)(pl + tot); - tot += sizeof(uint16_t); + //msgsz = *(uint16_t*)(pl + tot); + //tot += sizeof(uint16_t); /* fprintf(stderr, "[DEBUG] msg size = %u, tot = %zu\n", msgsz, tot); */ /* Make sure the message payload can be read */ - if(msgsz > 0 && msgsz <= MSG_SIZE - tot) { + // if(msgsz > 0 && msgsz <= MSG_SIZE - tot) { /* Append message to the Buzz input message queue */ - buzzinmsg_queue_append( - VM->inmsgs, - buzzmsg_payload_frombuffer(pl + tot, msgsz)); - tot += msgsz; + // buzzinmsg_queue_append( + // VM->inmsgs, + // buzzmsg_payload_frombuffer(pl + tot, msgsz)); + // tot += msgsz; /* fprintf(stderr, "[DEBUG] appended message, tot = %zu\n", tot); */ - } - } - while(MSG_SIZE - tot > sizeof(uint16_t) && msgsz > 0); + // } + //} + //while(MSG_SIZE - tot > sizeof(uint16_t) && msgsz > 0); /* Erase packet */ /* fprintf(stderr, "[DEBUG] Done processing packet %p from %d\n", */ /* PACKETS_FIRST, */ /* PACKETS_FIRST->id); */ - free(PACKETS_FIRST->payload); - free(PACKETS_FIRST); + //free(PACKETS_FIRST->payload); + //free(PACKETS_FIRST); /* Go to next packet */ - PACKETS_FIRST = n; - } + // PACKETS_FIRST = n; + //} /* The packet list is now empty */ - PACKETS_LAST = NULL; + //PACKETS_LAST = NULL; /* Unlock mutex */ - pthread_mutex_unlock(&INCOMING_PACKET_MUTEX); + //pthread_mutex_unlock(&INCOMING_PACKET_MUTEX); /* fprintf(stderr, "[DEBUG] Done processing incoming packets.\n"); */ /* Process messages */ - buzzvm_process_inmsgs(VM); + //buzzvm_process_inmsgs(VM); /* * Update sensors */ @@ -468,43 +470,44 @@ void buzz_script_step() { * Broadcast messages */ /* Prepare buffer */ - memset(STREAM_SEND_BUF, 0, MSG_SIZE); - *(uint16_t*)STREAM_SEND_BUF = VM->robot; - ssize_t tot = sizeof(uint16_t); - do { + // memset(STREAM_SEND_BUF, 0, MSG_SIZE); + // *(uint16_t*)STREAM_SEND_BUF = VM->robot; + // ssize_t tot = sizeof(uint16_t); + //do { /* Are there more messages? */ - if(buzzoutmsg_queue_isempty(VM->outmsgs)) break; + // if(buzzoutmsg_queue_isempty(VM->outmsgs)) break; /* Get first message */ - buzzmsg_payload_t m = buzzoutmsg_queue_first(VM->outmsgs); + // buzzmsg_payload_t m = buzzoutmsg_queue_first(VM->outmsgs); /* Make sure it fits the data buffer */ - if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t) - > - MSG_SIZE) { - buzzmsg_payload_destroy(&m); - break; - } + // if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t) + // > + // MSG_SIZE) { + // buzzmsg_payload_destroy(&m); + // break; + // } /* Add message length to data buffer */ /* fprintf(stderr, "[DEBUG] send before sz = %u\n", */ /* *(uint16_t*)(STREAM_SEND_BUF + 2)); */ - *(uint16_t*)(STREAM_SEND_BUF + tot) = (uint16_t)buzzmsg_payload_size(m); - tot += sizeof(uint16_t); + // *(uint16_t*)(STREAM_SEND_BUF + tot) = (uint16_t)buzzmsg_payload_size(m); + // tot += sizeof(uint16_t); /* fprintf(stderr, "[DEBUG] send after sz = %u\n", */ /* *(uint16_t*)(STREAM_SEND_BUF + 2)); */ /* Add payload to data buffer */ - memcpy(STREAM_SEND_BUF + tot, m->data, buzzmsg_payload_size(m)); - tot += buzzmsg_payload_size(m); + // memcpy(STREAM_SEND_BUF + tot, m->data, buzzmsg_payload_size(m)); + // tot += buzzmsg_payload_size(m); /* Get rid of message */ - buzzoutmsg_queue_next(VM->outmsgs); - buzzmsg_payload_destroy(&m); - } while(1); + // buzzoutmsg_queue_next(VM->outmsgs); + // buzzmsg_payload_destroy(&m); + // } while(1); + /* fprintf(stderr, "[DEBUG] send id = %u, sz = %u\n", */ /* *(uint16_t*)STREAM_SEND_BUF, */ /* *(uint16_t*)(STREAM_SEND_BUF + 2)); */ /* Send messages */ - buzzvm_process_outmsgs(VM); - STREAM_SEND(); + // buzzvm_process_outmsgs(VM); + // STREAM_SEND(); /* Sleep */ - usleep(100000); + // usleep(100000); /* Print swarm */ buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); /* Check swarm state */ diff --git a/rosbuzz/src/rosbuzz.cpp b/rosbuzz/src/rosbuzz.cpp index 76dee05..c73125d 100644 --- a/rosbuzz/src/rosbuzz.cpp +++ b/rosbuzz/src/rosbuzz.cpp @@ -1,29 +1,7 @@ /* - * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. + * Header + */ // %Tag(FULLTEXT)% // %Tag(ROS_HEADER)% @@ -64,6 +42,14 @@ void usage(const char* path, int status) { exit(status); } +// %Tag(CALLBACK)% +void chatterCallback(const std_msgs::String::ConstPtr& msg) +{ + ROS_INFO("I heard: [%s]", msg->data.c_str()); +} +// %EndTag(CALLBACK)% + + static void ctrlc_handler(int sig) { done = 1; } @@ -81,7 +67,8 @@ int main(int argc, char **argv) * part of the ROS system. */ // %Tag(INIT)% - ros::init(argc, argv, "talker"); + ros::init(argc, argv, "outgoing"); + ros::init(argc, argv, "incoming"); // %EndTag(INIT)% /** @@ -90,7 +77,8 @@ int main(int argc, char **argv) * NodeHandle destructed will close down the node. */ // %Tag(NODEHANDLE)% - ros::NodeHandle n; + ros::NodeHandle n_outgoing; + ros::NodeHandle n_incoming; // %EndTag(NODEHANDLE)% /** @@ -111,11 +99,15 @@ int main(int argc, char **argv) * buffer up before throwing some away. */ // %Tag(PUBLISHER)% - ros::Publisher chatter_pub = n.advertise("chatter", 1000); + ros::Publisher outgoing_pub = n_outgoing.advertise("outgoing", 1000); // %EndTag(PUBLISHER)% +// %Tag(SUBSCRIBER)% + ros::Subscriber sub = n_incoming.subscribe("incoming", 1000, chatterCallback); +// %EndTag(SUBSCRIBER)% + // %Tag(LOOP_RATE)% - ros::Rate loop_rate(10); + ros::Rate loop_rate(2); // %EndTag(LOOP_RATE)% /** @@ -125,10 +117,7 @@ int main(int argc, char **argv) // %Tag(ROS_OK)% - int count = 0; - while (ros::ok()) - { - +// buzz setting // Buzz stuff // @@ -140,16 +129,16 @@ int main(int argc, char **argv) // /* Parse command line */ - if(argc != 5) usage(argv[0], 0); + /* if(argc != 5) usage(argv[0], 0); */ /* The stream type */ - char* stream = argv[1]; + /* char* stream = argv[1]; if(strcmp(stream, "tcp") != 0 && strcmp(stream, "bt") != 0) { fprintf(stderr, "%s: unknown stream type '%s'\n", argv[0], stream); usage(argv[0], 0); } /* The message size */ - char* endptr; + /*char* endptr; int msg_sz = strtol(argv[2], &endptr, 10); if(endptr == argv[2] || *endptr != '\0') { fprintf(stderr, "%s: can't parse '%s' into a number\n", argv[0], argv[2]); @@ -158,44 +147,35 @@ int main(int argc, char **argv) if(msg_sz <= 0) { fprintf(stderr, "%s: invalid value %d for message size\n", argv[0], msg_sz); return 0; - } + } */ /* The bytecode filename */ - char* bcfname = argv[3]; + char* bcfname = "/home/vivek/catkin_ws/src/rosbuzz/src/out.bo"; //argv[1]; /* The debugging information file name */ - char* dbgfname = argv[4]; + char* dbgfname = "/home/vivek/catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2]; /* Wait for connection */ - if(!buzz_listen(stream, msg_sz)) return 1; + //if(!buzz_listen(stream, msg_sz)) return 1; /* Set CTRL-C handler */ signal(SIGTERM, ctrlc_handler); signal(SIGINT, ctrlc_handler); /* Initialize the robot */ - kh4_setup(); + //kh4_setup(); /* Set the Buzz bytecode */ if(buzz_script_set(bcfname, dbgfname)) { + fprintf(stdout, "Bytecode file found and set\n"); + +// buzz setting + + int count = 0; + while (ros::ok() && !done && !buzz_script_done()) + { + + + // while(!done && !buzz_script_done()) /* Main loop */ - while(!done && !buzz_script_done()) buzz_script_step(); /* Cleanup */ - buzz_script_destroy(); - } + // buzz_script_destroy(); - /* Stop the robot */ - kh4_done(); - /* All done */ - return 0; - -// - - - - - - - - - - -// Buzz stuff // %EndTag(ROS_OK)% @@ -220,22 +200,27 @@ int main(int argc, char **argv) * given as a template parameter to the advertise<>() call, as was done * in the constructor above. */ + // %Tag(PUBLISH)% - chatter_pub.publish(msg); + outgoing_pub.publish(msg); // %EndTag(PUBLISH)% + // %Tag(SPINONCE)% ros::spinOnce(); // %EndTag(SPINONCE)% - // %Tag(RATE_SLEEP)% loop_rate.sleep(); // %EndTag(RATE_SLEEP)% ++count; } + /* Stop the robot */ + kh4_done(); + /* All done */ + return 0; +} + - - return 0; } // %EndTag(FULLTEXT)%