diff --git a/rosbuzz/CMakeLists.txt b/rosbuzz/CMakeLists.txt
new file mode 100644
index 0000000..f70cd76
--- /dev/null
+++ b/rosbuzz/CMakeLists.txt
@@ -0,0 +1,199 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rosbuzz)
+
+
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ roscpp
+ std_msgs
+)
+
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# st_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES buzzros
+# CATKIN_DEPENDS Buzz roscpp st_msgs
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(buzzros
+# src/${PROJECT_NAME}/buzzros.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(buzzros ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+# add_executable(buzzros_node src/buzzros_node.cpp)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(buzzros_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(buzzros_node
+# ${catkin_LIBRARIES}
+# )
+
+add_executable(rosbuzz_node src/rosbuzz.cpp
+ src/buzz_utility.c
+ src/buzzkh4_closures.c
+ src/kh4_utility.c)
+target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} /usr/local/lib/libbuzz.so /usr/local/lib/libbuzzdbg.so -lpthread)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+install(TARGETS rosbuzz_node
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_buzzros.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
+
+
+
diff --git a/rosbuzz/package.xml b/rosbuzz/package.xml
new file mode 100644
index 0000000..d47c4cc
--- /dev/null
+++ b/rosbuzz/package.xml
@@ -0,0 +1,50 @@
+
+
+ rosbuzz
+ 0.0.0
+ The rosbuzz package
+
+
+
+
+ vivek
+
+
+
+
+
+ TODO
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/rosbuzz/src/buzz_utility.c b/rosbuzz/src/buzz_utility.c
new file mode 100644
index 0000000..98e3eab
--- /dev/null
+++ b/rosbuzz/src/buzz_utility.c
@@ -0,0 +1,557 @@
+#define _GNU_SOURCE
+#include
+
+#include "buzz_utility.h"
+#include "buzzkh4_closures.h"
+#include
+
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+/****************************************/
+/****************************************/
+
+static buzzvm_t VM = 0;
+static char* BO_FNAME = 0;
+static uint8_t* BO_BUF = 0;
+static buzzdebug_t DBG_INFO = 0;
+static int MSG_SIZE = -1;
+static int TCP_LIST_STREAM = -1;
+static int TCP_COMM_STREAM = -1;
+static uint8_t* STREAM_SEND_BUF = NULL;
+
+#define TCP_LIST_STREAM_PORT "24580"
+
+/* Pointer to a function that sends a message on the stream */
+static void (*STREAM_SEND)() = NULL;
+
+/* PThread handle to manage incoming messages */
+static pthread_t INCOMING_MSG_THREAD;
+
+/****************************************/
+/****************************************/
+
+/* PThread mutex to manage the list of incoming packets */
+static pthread_mutex_t INCOMING_PACKET_MUTEX;
+
+/* List of packets received over the stream */
+struct incoming_packet_s {
+ /* Id of the message sender */
+ int id;
+ /* Payload */
+ uint8_t* payload;
+ /* Next message */
+ struct incoming_packet_s* next;
+};
+
+/* The list of incoming packets */
+static struct incoming_packet_s* PACKETS_FIRST = NULL;
+static struct incoming_packet_s* PACKETS_LAST = NULL;
+
+void incoming_packet_add(int id,
+ const uint8_t* pl) {
+ /* Create packet */
+ struct incoming_packet_s* p =
+ (struct incoming_packet_s*)malloc(sizeof(struct incoming_packet_s));
+ /* Fill in the data */
+ p->id = id;
+ p->payload = malloc(MSG_SIZE - sizeof(uint16_t));
+ memcpy(p->payload, pl, MSG_SIZE - sizeof(uint16_t));
+ p->next = NULL;
+ /* Lock mutex */
+ pthread_mutex_lock(&INCOMING_PACKET_MUTEX);
+ /* Add as last to list */
+ if(PACKETS_FIRST != NULL)
+ PACKETS_LAST->next = p;
+ else
+ PACKETS_FIRST = p;
+ PACKETS_LAST = p;
+ /* Unlock mutex */
+ pthread_mutex_unlock(&INCOMING_PACKET_MUTEX);
+ /* fprintf(stderr, "[DEBUG] Added packet from %d\n", id); */
+}
+
+/****************************************/
+/****************************************/
+
+void* buzz_stream_incoming_thread_tcp(void* args) {
+ /* Create buffer for message */
+ uint8_t* buf = calloc(MSG_SIZE, 1);
+ /* Tot bytes left to receive, received up to now, and received at a
+ * specific call of recv() */
+ ssize_t left, tot, cur;
+ while(1) {
+ /* Initialize left byte count */
+ left = MSG_SIZE;
+ tot = 0;
+ while(left > 0) {
+ cur = recv(TCP_COMM_STREAM, buf + tot, left, 0);
+ /* fprintf(stderr, "[DEBUG] Received %zd bytes", cur); */
+ if(cur < 0) {
+ fprintf(stderr, "Error receiving data: %s\n", strerror(errno));
+ free(buf);
+ return NULL;
+ }
+ if(cur == 0) {
+ fprintf(stderr, "Connection closed by peer\n");
+ free(buf);
+ return NULL;
+ }
+ left -= cur;
+ tot += cur;
+ /* fprintf(stderr, ", %zd left, %zd tot\n", left, tot); */
+ }
+ /* Done receiving data, add packet to list */
+ incoming_packet_add(*(uint16_t*)buf,
+ buf + sizeof(uint16_t));
+ }
+}
+
+void buzz_stream_send_tcp() {
+ /* Tot bytes left to send, sent up to now, and sent at a specific
+ * call of send() */
+ ssize_t left, tot, cur;
+ /* Initialize left byte count */
+ left = MSG_SIZE;
+ tot = 0;
+ while(left > 0) {
+ cur = send(TCP_COMM_STREAM, STREAM_SEND_BUF + tot, left, 0);
+ /* fprintf(stderr, "[DEBUG] Sent %zd bytes", cur); */
+ if(cur < 0) {
+ fprintf(stderr, "Error receiving data: %s\n", strerror(errno));
+ exit(1);
+ }
+ if(cur == 0) {
+ fprintf(stderr, "Connection closed by peer\n");
+ exit(1);
+ }
+ left -= cur;
+ tot += cur;
+ /* fprintf(stderr, ", %zd left, %zd tot\n", left, tot); */
+ }
+}
+
+/****************************************/
+/****************************************/
+
+int buzz_listen_tcp() {
+ /* Used to store the return value of the network function calls */
+ int retval;
+ /* Get information on the available interfaces */
+ struct addrinfo hints, *ifaceinfo;
+ memset(&hints, 0, sizeof(hints));
+ hints.ai_family = AF_INET; /* Only IPv4 is accepted */
+ hints.ai_socktype = SOCK_STREAM; /* TCP socket */
+ hints.ai_flags = AI_PASSIVE; /* Necessary for bind() later on */
+ retval = getaddrinfo(NULL,
+ TCP_LIST_STREAM_PORT,
+ &hints,
+ &ifaceinfo);
+ if(retval != 0) {
+ fprintf(stderr, "Error getting local address information: %s\n",
+ gai_strerror(retval));
+ return 0;
+ }
+ /* Bind on the first interface available */
+ TCP_LIST_STREAM = -1;
+ struct addrinfo* iface = NULL;
+ for(iface = ifaceinfo;
+ (iface != NULL) && (TCP_LIST_STREAM == -1);
+ iface = iface->ai_next) {
+ TCP_LIST_STREAM = socket(iface->ai_family,
+ iface->ai_socktype,
+ iface->ai_protocol);
+ if(TCP_LIST_STREAM > 0) {
+ int true = 1;
+ if((setsockopt(TCP_LIST_STREAM,
+ SOL_SOCKET,
+ SO_REUSEADDR,
+ &true,
+ sizeof(true)) != -1)
+ &&
+ (bind(TCP_LIST_STREAM,
+ iface->ai_addr,
+ iface->ai_addrlen) == -1)) {
+ close(TCP_LIST_STREAM);
+ TCP_LIST_STREAM = -1;
+ }
+ }
+ }
+ freeaddrinfo(ifaceinfo);
+ if(TCP_LIST_STREAM == -1) {
+ fprintf(stderr, "Can't bind socket to any interface\n");
+ return 0;
+ }
+ /* Listen on the socket */
+ fprintf(stdout, "Listening on port " TCP_LIST_STREAM_PORT "...\n");
+ if(listen(TCP_LIST_STREAM, 1) == -1) {
+ close(TCP_LIST_STREAM);
+ TCP_LIST_STREAM = -1;
+ fprintf(stderr, "Can't listen on the socket: %s\n",
+ strerror(errno));
+ return 0;
+ }
+ /* Accept incoming connection */
+ struct sockaddr addr;
+ socklen_t addrlen = sizeof(addr);
+ TCP_COMM_STREAM = accept(TCP_LIST_STREAM, &addr, &addrlen);
+ if(TCP_COMM_STREAM == -1) {
+ close(TCP_LIST_STREAM);
+ TCP_LIST_STREAM = -1;
+ fprintf(stderr, "Error accepting connection: %s\n",
+ strerror(errno));
+ return 0;
+ }
+ fprintf(stdout, "Accepted connection from %s\n",
+ inet_ntoa(((struct sockaddr_in*)(&addr))->sin_addr));
+ /* Ready to communicate through TCP */
+ STREAM_SEND = buzz_stream_send_tcp;
+ STREAM_SEND_BUF = (uint8_t*)malloc(MSG_SIZE);
+ if(pthread_create(&INCOMING_MSG_THREAD, NULL, &buzz_stream_incoming_thread_tcp, NULL) != 0) {
+ fprintf(stderr, "Can't create thread: %s\n", strerror(errno));
+ close(TCP_COMM_STREAM);
+ TCP_COMM_STREAM = -1;
+ return 0;
+ }
+ return 1;
+}
+
+int buzz_listen_bt() {
+ return 0;
+}
+
+int buzz_listen(const char* type,
+ int msg_size) {
+ /* Set the message size */
+ MSG_SIZE = msg_size;
+ /* Create the mutex */
+ if(pthread_mutex_init(&INCOMING_PACKET_MUTEX, NULL) != 0) {
+ fprintf(stderr, "Error initializing the incoming packet mutex: %s\n",
+ strerror(errno));
+ return 0;
+ }
+ /* Listen to connections */
+ if(strcmp(type, "tcp") == 0)
+ return buzz_listen_tcp();
+ else if(strcmp(type, "bt") == 0)
+ return buzz_listen_bt();
+ return 0;
+}
+
+/****************************************/
+/****************************************/
+
+static const char* buzz_error_info() {
+ buzzdebug_entry_t dbg = *buzzdebug_info_get_fromoffset(DBG_INFO, &VM->pc);
+ char* msg;
+ if(dbg != NULL) {
+ asprintf(&msg,
+ "%s: execution terminated abnormally at %s:%" PRIu64 ":%" PRIu64 " : %s\n\n",
+ BO_FNAME,
+ dbg->fname,
+ dbg->line,
+ dbg->col,
+ VM->errormsg);
+ }
+ else {
+ asprintf(&msg,
+ "%s: execution terminated abnormally at bytecode offset %d: %s\n\n",
+ BO_FNAME,
+ VM->pc,
+ VM->errormsg);
+ }
+ return msg;
+}
+
+/****************************************/
+/****************************************/
+
+static int buzz_register_hooks() {
+ buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
+ buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzkh4_print));
+ buzzvm_gstore(VM);
+ buzzvm_pushs(VM, buzzvm_string_register(VM, "set_wheels", 1));
+ buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzkh4_set_wheels));
+ buzzvm_gstore(VM);
+ buzzvm_pushs(VM, buzzvm_string_register(VM, "set_leds", 1));
+ buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzkh4_set_leds));
+ buzzvm_gstore(VM);
+ return BUZZVM_STATE_READY;
+}
+
+/****************************************/
+/****************************************/
+
+int buzz_script_set(const char* bo_filename,
+ const char* bdbg_filename) {
+ /* Get hostname */
+ char hstnm[30];
+ gethostname(hstnm, 30);
+ /* Make numeric id from hostname */
+ /* NOTE: here we assume that the hostname is in the format Knn */
+ int id = strtol(hstnm + 1, NULL, 10);
+ /* Reset the Buzz VM */
+ if(VM) buzzvm_destroy(&VM);
+ VM = buzzvm_new(id);
+ /* Get rid of debug info */
+ if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
+ DBG_INFO = buzzdebug_new();
+ /* Read bytecode and fill in data structure */
+ FILE* fd = fopen(bo_filename, "rb");
+ if(!fd) {
+ perror(bo_filename);
+ return 0;
+ }
+ fseek(fd, 0, SEEK_END);
+ size_t bcode_size = ftell(fd);
+ rewind(fd);
+ BO_BUF = (uint8_t*)malloc(bcode_size);
+ if(fread(BO_BUF, 1, bcode_size, fd) < bcode_size) {
+ perror(bo_filename);
+ buzzvm_destroy(&VM);
+ buzzdebug_destroy(&DBG_INFO);
+ fclose(fd);
+ return 0;
+ }
+ fclose(fd);
+ /* Read debug information */
+ if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
+ buzzvm_destroy(&VM);
+ buzzdebug_destroy(&DBG_INFO);
+ perror(bdbg_filename);
+ return 0;
+ }
+ /* Set byte code */
+ if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
+ buzzvm_destroy(&VM);
+ buzzdebug_destroy(&DBG_INFO);
+ fprintf(stdout, "%s: Error loading Buzz script\n\n", bo_filename);
+ return 0;
+ }
+ /* Register hook functions */
+ if(buzz_register_hooks() != BUZZVM_STATE_READY) {
+ buzzvm_destroy(&VM);
+ buzzdebug_destroy(&DBG_INFO);
+ fprintf(stdout, "%s: Error registering hooks\n\n", bo_filename);
+ return 0;
+ }
+ /* Save bytecode file name */
+ BO_FNAME = strdup(bo_filename);
+ /* Execute the global part of the script */
+ buzzvm_execute_script(VM);
+ /* Call the Init() function */
+ buzzvm_function_call(VM, "init", 0);
+ /* All OK */
+ return 1;
+}
+
+/****************************************/
+/****************************************/
+
+struct buzzswarm_elem_s {
+ buzzdarray_t swarms;
+ uint16_t age;
+};
+typedef struct buzzswarm_elem_s* buzzswarm_elem_t;
+
+void check_swarm_members(const void* key, void* data, void* params) {
+ buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
+ int* status = (int*)params;
+ if(*status == 3) return;
+ if(buzzdarray_size(e->swarms) != 1) {
+ fprintf(stderr, "Swarm list size is not 1\n");
+ *status = 3;
+ }
+ else {
+ int sid = 1;
+ if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
+ buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
+ fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
+ sid,
+ buzzdarray_get(e->swarms, 0, uint16_t));
+ *status = 3;
+ return;
+ }
+ sid = 2;
+ if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
+ buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
+ fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
+ sid,
+ buzzdarray_get(e->swarms, 0, uint16_t));
+ *status = 3;
+ return;
+ }
+ }
+}
+
+void buzz_script_step() {
+
+
+ /*
+ * Process incoming messages
+ */
+ /* Reset neighbor information */
+ buzzneighbors_reset(VM);
+ /* Lock mutex */
+ /* fprintf(stderr, "[DEBUG] Processing incoming packets...\n"); */
+ //pthread_mutex_lock(&INCOMING_PACKET_MUTEX);
+ /* Go through messages and add them to the FIFO */
+ //struct incoming_packet_s* n;
+ //while(PACKETS_FIRST) {
+ /* Save next packet */
+ // n = PACKETS_FIRST->next;
+ /* Update Buzz neighbors information */
+ // 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;
+ 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 {
+ /* Get payload size */
+ //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) {
+ /* Append message to the Buzz input message queue */
+ // 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);
+ /* Erase packet */
+ /* fprintf(stderr, "[DEBUG] Done processing packet %p from %d\n", */
+ /* PACKETS_FIRST, */
+ /* PACKETS_FIRST->id); */
+ //free(PACKETS_FIRST->payload);
+ //free(PACKETS_FIRST);
+ /* Go to next packet */
+ // PACKETS_FIRST = n;
+ //}
+ /* The packet list is now empty */
+ //PACKETS_LAST = NULL;
+ /* Unlock mutex */
+ //pthread_mutex_unlock(&INCOMING_PACKET_MUTEX);
+ /* fprintf(stderr, "[DEBUG] Done processing incoming packets.\n"); */
+ /* Process messages */
+ //buzzvm_process_inmsgs(VM);
+ /*
+ * Update sensors
+ */
+ buzzkh4_update_battery(VM);
+ buzzkh4_update_ir(VM);
+ /*
+ * Call Buzz step() function
+ */
+ if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) {
+ fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
+ BO_FNAME,
+ buzz_error_info());
+ buzzvm_dump(VM);
+ }
+ /*
+ * 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 {
+ /* Are there more messages? */
+ // if(buzzoutmsg_queue_isempty(VM->outmsgs)) break;
+ /* Get first message */
+ // 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;
+ // }
+ /* 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);
+ /* 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);
+ /* Get rid of message */
+ // 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();
+ /* Sleep */
+ // usleep(100000);
+ /* Print swarm */
+ buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
+ /* Check swarm state */
+ int status = 1;
+ buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
+ if(status == 1 &&
+ buzzdict_size(VM->swarmmembers) < 9)
+ status = 2;
+ buzzvm_pushs(VM, buzzvm_string_register(VM, "swarm_status", 1));
+ buzzvm_pushi(VM, status);
+ buzzvm_gstore(VM);
+}
+
+/****************************************/
+/****************************************/
+
+void buzz_script_destroy() {
+ /* Cancel thread */
+ pthread_cancel(INCOMING_MSG_THREAD);
+ pthread_join(INCOMING_MSG_THREAD, NULL);
+ /* Get rid of stream buffer */
+ free(STREAM_SEND_BUF);
+ /* Get rid of virtual machine */
+ if(VM) {
+ if(VM->state != BUZZVM_STATE_READY) {
+ fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
+ BO_FNAME,
+ buzz_error_info());
+ buzzvm_dump(VM);
+ }
+ buzzvm_function_call(VM, "destroy", 0);
+ buzzvm_destroy(&VM);
+ free(BO_FNAME);
+ buzzdebug_destroy(&DBG_INFO);
+ }
+ fprintf(stdout, "Script execution stopped.\n");
+}
+
+/****************************************/
+/****************************************/
+
+int buzz_script_done() {
+ return VM->state != BUZZVM_STATE_READY;
+}
+
+/****************************************/
+/****************************************/
diff --git a/rosbuzz/src/buzz_utility.h b/rosbuzz/src/buzz_utility.h
new file mode 100644
index 0000000..be3cb3d
--- /dev/null
+++ b/rosbuzz/src/buzz_utility.h
@@ -0,0 +1,16 @@
+#ifndef BUZZ_UTILITY_H
+#define BUZZ_UTILITY_H
+
+extern int buzz_listen(const char* type,
+ int msg_size);
+
+extern int buzz_script_set(const char* bo_filename,
+ const char* bdbg_filename);
+
+extern void buzz_script_step();
+
+extern void buzz_script_destroy();
+
+extern int buzz_script_done();
+
+#endif
diff --git a/rosbuzz/src/buzzasm.h b/rosbuzz/src/buzzasm.h
new file mode 100644
index 0000000..69e71f6
--- /dev/null
+++ b/rosbuzz/src/buzzasm.h
@@ -0,0 +1,56 @@
+#ifndef BUZZASM_H
+#define BUZZASM_H
+
+#include
+#include
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+ /*
+ * Compiles an assembly file into bytecode.
+ * @param fname The file name where the code is located.
+ * @param buf The buffer in which the bytecode will be stored. Created internally.
+ * @param size The size of the bytecode buffer.
+ * @param dbg The debug data structure to fill into. Created internally.
+ * @return 0 if no error occurred, 1 for I/O error, 2 for compilation error.
+ */
+ extern int buzz_asm(const char* fname,
+ uint8_t** buf,
+ uint32_t* size,
+ buzzdebug_t* dbg);
+
+ /*
+ * Decompiles bytecode into an assembly file.
+ * @param buf The buffer in which the bytecode is stored.
+ * @param size The size of the bytecode buffer.
+ * @param dbg The debug data structure.
+ * @param fname The file name where the assembly will be written.
+ * @return 0 if no error occurred, 1 for I/O error, 2 for decompilation error.
+ */
+ extern int buzz_deasm(const uint8_t* buf,
+ uint32_t size,
+ buzzdebug_t dbg,
+ const char* fname);
+
+ /*
+ * Decompiles the bytecode of a single instruction.
+ * This function writes the decompiled instruction into a new string pointed to by
+ * *buf. When you're done with it, you must free it.
+ * NOTE: no sanity check is performed to make sure that this function does not
+ * read beyond the bcode buffer size.
+ * @param bcode The buffer in which the bytecode is stored.
+ * @param off The offset at which decompilation must occur.
+ * @param buf The bytecode where the
+ * @return 1 if no error occurred, 0 for decompilation error.
+ */
+ extern int buzz_instruction_deasm(const uint8_t* bcode,
+ uint32_t off,
+ char** buf);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/rosbuzz/src/buzzkh4_closures.c b/rosbuzz/src/buzzkh4_closures.c
new file mode 100644
index 0000000..8d10c24
--- /dev/null
+++ b/rosbuzz/src/buzzkh4_closures.c
@@ -0,0 +1,137 @@
+#define _GNU_SOURCE
+#include
+#include "buzzkh4_closures.h"
+#include "kh4_utility.h"
+
+/****************************************/
+/****************************************/
+
+int buzzkh4_print(buzzvm_t vm) {
+ int i;
+ for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
+ buzzvm_lload(vm, i);
+ buzzobj_t o = buzzvm_stack_at(vm, 1);
+ buzzvm_pop(vm);
+ switch(o->o.type) {
+ case BUZZTYPE_NIL:
+ fprintf(stdout, "[nil]");
+ break;
+ case BUZZTYPE_INT:
+ fprintf(stdout, "%d", o->i.value);
+ break;
+ case BUZZTYPE_FLOAT:
+ fprintf(stdout, "%f", o->f.value);
+ break;
+ case BUZZTYPE_TABLE:
+ fprintf(stdout, "[table with %d elems]", (buzzdict_size(o->t.value)));
+ break;
+ case BUZZTYPE_CLOSURE:
+ if(o->c.value.isnative)
+ fprintf(stdout, "[n-closure @%d]", o->c.value.ref);
+ else
+ fprintf(stdout, "[c-closure @%d]", o->c.value.ref);
+ break;
+ case BUZZTYPE_STRING:
+ fprintf(stdout, "%s", o->s.value.str);
+ break;
+ case BUZZTYPE_USERDATA:
+ fprintf(stdout, "[userdata @%p]", o->u.value);
+ break;
+ default:
+ break;
+ }
+ }
+ fprintf(stdout, "\n");
+ return buzzvm_ret0(vm);
+}
+
+/****************************************/
+/****************************************/
+
+int buzzkh4_set_wheels(buzzvm_t vm) {
+ buzzvm_lnum_assert(vm, 2);
+ buzzvm_lload(vm, 1); /* Left speed */
+ buzzvm_lload(vm, 2); /* Right speed */
+ buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
+ buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
+ // kh4_set_speed(buzzvm_stack_at(vm, 2)->f.value * 10.0f, /* Left speed */
+ // buzzvm_stack_at(vm, 1)->f.value * 10.0f, /* Right speed */
+ // DSPIC);
+ return buzzvm_ret0(vm);
+}
+
+/****************************************/
+/****************************************/
+
+int buzzkh4_set_leds(buzzvm_t vm) {
+ buzzvm_lnum_assert(vm, 3);
+ buzzvm_lload(vm, 1); /* Red */
+ buzzvm_lload(vm, 2); /* Green */
+ buzzvm_lload(vm, 3); /* Blue */
+ buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
+ buzzvm_type_assert(vm, 2, BUZZTYPE_INT);
+ buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
+ int32_t r = buzzvm_stack_at(vm, 3)->i.value;
+ int32_t g = buzzvm_stack_at(vm, 2)->i.value;
+ int32_t b = buzzvm_stack_at(vm, 1)->i.value;
+ //kh4_SetRGBLeds(r,g,b, /* Left */
+ // r,g,b, /* Right */
+ // r,g,b, /* Back */
+ // DSPIC);
+ return buzzvm_ret0(vm);
+}
+
+/****************************************/
+/****************************************/
+
+int buzzkh4_update_battery(buzzvm_t vm) {
+ static char BATTERY_BUF[256];
+ // kh4_battery_status(BATTERY_BUF, DSPIC);
+ buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1));
+ buzzvm_pusht(vm);
+ buzzvm_dup(vm);
+ buzzvm_pushs(vm, buzzvm_string_register(vm, "voltage", 1));
+ buzzvm_pushf(vm, (BATTERY_BUF[10] | BATTERY_BUF[11] << 8) * .00975f);
+ buzzvm_tput(vm);
+ buzzvm_dup(vm);
+ buzzvm_pushs(vm, buzzvm_string_register(vm, "current", 1));
+ buzzvm_pushf(vm, (BATTERY_BUF[6] | BATTERY_BUF[7] << 8) * .07813f);
+ buzzvm_tput(vm);
+ buzzvm_dup(vm);
+ buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1));
+ buzzvm_pushf(vm, BATTERY_BUF[3]);
+ buzzvm_tput(vm);
+ buzzvm_gstore(vm);
+ return vm->state;
+}
+
+/****************************************/
+/****************************************/
+
+int buzzkh4_update_ir(buzzvm_t vm) {
+ static char PROXIMITY_BUF[256];
+ int i;
+ //kh4_proximity_ir(PROXIMITY_BUF, DSPIC);
+ buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity_ir", 1));
+ buzzvm_pusht(vm);
+ for(i = 0; i < 8; i++) {
+ buzzvm_dup(vm);
+ buzzvm_pushi(vm, i+1);
+ buzzvm_pushi(vm, (PROXIMITY_BUF[i*2] | PROXIMITY_BUF[i*2+1] << 8));
+ buzzvm_tput(vm);
+ }
+ buzzvm_gstore(vm);
+ buzzvm_pushs(vm, buzzvm_string_register(vm, "ground_ir", 1));
+ buzzvm_pusht(vm);
+ for(i = 8; i < 12; i++) {
+ buzzvm_dup(vm);
+ buzzvm_pushi(vm, i-7);
+ buzzvm_pushi(vm, (PROXIMITY_BUF[i*2] | PROXIMITY_BUF[i*2+1] << 8));
+ buzzvm_tput(vm);
+ }
+ buzzvm_gstore(vm);
+ return vm->state;
+}
+
+/****************************************/
+/****************************************/
diff --git a/rosbuzz/src/buzzkh4_closures.h b/rosbuzz/src/buzzkh4_closures.h
new file mode 100644
index 0000000..f3ea2f4
--- /dev/null
+++ b/rosbuzz/src/buzzkh4_closures.h
@@ -0,0 +1,36 @@
+#ifndef BUZZKH4_CLOSURES_H
+#define BUZZKH4_CLOSURES_H
+
+#include
+
+/*
+ * prextern int() function in Buzz
+ */
+extern int buzzkh4_print(buzzvm_t vm);
+
+/*
+ * set_wheels(ls,rs) function in Buzz
+ * Sets the wheel speeds to ls (left) and rs (right)
+ * speeds are expressed in cm/s
+ */
+extern int buzzkh4_set_wheels(buzzvm_t vm);
+
+/*
+ * set_leds(r,g,b) function in Buzz
+ * Sets the color of the 3 leds to (r,g,b)
+ * speeds are expressed in cm/s
+ */
+extern int buzzkh4_set_leds(buzzvm_t vm);
+
+/*
+ * Updates battery information in Buzz
+ */
+extern int buzzkh4_update_battery(buzzvm_t vm);
+
+/*
+ * Updates IR information in Buzz
+ * Proximity and ground sensors
+ */
+extern int buzzkh4_update_ir(buzzvm_t vm);
+
+#endif
diff --git a/rosbuzz/src/kh4_utility.c b/rosbuzz/src/kh4_utility.c
new file mode 100644
index 0000000..0675fb6
--- /dev/null
+++ b/rosbuzz/src/kh4_utility.c
@@ -0,0 +1,55 @@
+#include "kh4_utility.h"
+#include "stdio.h"
+
+/****************************************/
+/****************************************/
+
+//knet_dev_t* DSPIC;
+static const int ACC_INC = 3;
+static const int ACC_DIV = 0;
+static const int MIN_SPEED_ACC = 20;
+static const int MIN_SPEED_DEC = 1;
+static const int MAX_SPEED = 400; /* mm/sec */
+
+/****************************************/
+/****************************************/
+
+void kh4_setup() {
+ /* Set the libkhepera debug level */
+ /*kb_set_debug_level(2);
+ // initiate libkhepera and robot access
+ if(kh4_init(0, NULL) !=0)
+ {
+ printf("\nERROR: could not initialize the khepera!\n\n");
+ return;
+ }
+ /* open robot socket and store the handle in their respective pointers
+ DSPIC = knet_open("Khepera4:dsPic", KNET_BUS_I2C, 0, NULL);
+ /* Set speed profile
+ kh4_SetSpeedProfile(ACC_INC, /* Acceleration increment
+ ACC_DIV, /* Acceleration divider
+ MIN_SPEED_ACC, /* Minimum speed acc
+ MIN_SPEED_DEC, /* Minimum speed dec
+ MAX_SPEED, /* Maximum speed
+ DSPIC
+ );
+ kh4_SetMode(kh4RegSpeedProfile, DSPIC);
+ /* Mute ultrasonic sensor
+ kh4_activate_us(0, DSPIC);*/
+}
+
+/****************************************/
+/****************************************/
+
+void kh4_done() {
+ /* /* Stop wheels
+ kh4_set_speed(0, 0, DSPIC);
+ /* Set motors to idle
+ kh4_SetMode(kh4RegIdle, DSPIC);
+ /* Clear rgb leds because they consume energy
+ kh4_SetRGBLeds(0, 0, 0, 0, 0, 0, 0, 0, 0, DSPIC); */
+ fprintf(stdout, "Robot stopped.\n");
+}
+
+/****************************************/
+/****************************************/
diff --git a/rosbuzz/src/kh4_utility.h b/rosbuzz/src/kh4_utility.h
new file mode 100644
index 0000000..3ededb3
--- /dev/null
+++ b/rosbuzz/src/kh4_utility.h
@@ -0,0 +1,12 @@
+#ifndef KH4_UTILITY_H
+#define KH4_UTILITY_H
+
+//#include
+
+extern void kh4_setup();
+
+extern void kh4_done();
+
+//extern knet_dev_t* DSPIC;
+
+#endif
diff --git a/rosbuzz/src/rosbuzz.cpp b/rosbuzz/src/rosbuzz.cpp
new file mode 100644
index 0000000..c73125d
--- /dev/null
+++ b/rosbuzz/src/rosbuzz.cpp
@@ -0,0 +1,226 @@
+/*
+
+ * Header
+
+ */
+// %Tag(FULLTEXT)%
+// %Tag(ROS_HEADER)%
+#include "ros/ros.h"
+// %EndTag(ROS_HEADER)%
+// %Tag(MSG_HEADER)%
+#include "std_msgs/String.h"
+// %EndTag(MSG_HEADER)%
+
+#include
+#include
+extern "C" {
+#include "buzz_utility.h"
+}
+extern "C" {
+#include "kh4_utility.h"
+}
+#include
+#include
+#include
+
+static int done = 0;
+
+/**
+ * This tutorial demonstrates simple sending of messages over the ROS system.
+ */
+/*
+ * Print usage information
+ */
+void usage(const char* path, int status) {
+ fprintf(stderr, "Usage:\n");
+ fprintf(stderr, "\t%s \n\n", path);
+ fprintf(stderr, "== Options ==\n\n");
+ fprintf(stderr, " stream The stream type: tcp or bt\n");
+ fprintf(stderr, " msg_size The message size in bytes\n");
+ fprintf(stderr, " file.bo The Buzz bytecode file\n");
+ fprintf(stderr, " file.bdbg The Buzz debug file\n\n");
+ 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;
+}
+
+int main(int argc, char **argv)
+{
+ /**
+ * The ros::init() function needs to see argc and argv so that it can perform
+ * any ROS arguments and name remapping that were provided at the command line.
+ * For programmatic remappings you can use a different version of init() which takes
+ * remappings directly, but for most command-line programs, passing argc and argv is
+ * the easiest way to do it. The third argument to init() is the name of the node.
+ *
+ * You must call one of the versions of ros::init() before using any other
+ * part of the ROS system.
+ */
+// %Tag(INIT)%
+ ros::init(argc, argv, "outgoing");
+ ros::init(argc, argv, "incoming");
+// %EndTag(INIT)%
+
+ /**
+ * NodeHandle is the main access point to communications with the ROS system.
+ * The first NodeHandle constructed will fully initialize this node, and the last
+ * NodeHandle destructed will close down the node.
+ */
+// %Tag(NODEHANDLE)%
+ ros::NodeHandle n_outgoing;
+ ros::NodeHandle n_incoming;
+// %EndTag(NODEHANDLE)%
+
+ /**
+ * The advertise() function is how you tell ROS that you want to
+ * publish on a given topic name. This invokes a call to the ROS
+ * master node, which keeps a registry of who is publishing and who
+ * is subscribing. After this advertise() call is made, the master
+ * node will notify anyone who is trying to subscribe to this topic name,
+ * and they will in turn negotiate a peer-to-peer connection with this
+ * node. advertise() returns a Publisher object which allows you to
+ * publish messages on that topic through a call to publish(). Once
+ * all copies of the returned Publisher object are destroyed, the topic
+ * will be automatically unadvertised.
+ *
+ * The second parameter to advertise() is the size of the message queue
+ * used for publishing messages. If messages are published more quickly
+ * than we can send them, the number here specifies how many messages to
+ * buffer up before throwing some away.
+ */
+// %Tag(PUBLISHER)%
+ 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(2);
+// %EndTag(LOOP_RATE)%
+
+ /**
+ * A count of how many messages we have sent. This is used to create
+ * a unique string for each message.
+ */
+// %Tag(ROS_OK)%
+
+
+// buzz setting
+
+// Buzz stuff
+//
+
+
+
+
+
+
+//
+/* Parse command line */
+ /* if(argc != 5) usage(argv[0], 0); */
+ /* The stream type */
+ /* 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;
+ 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]);
+ return 0;
+ }
+ 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 = "/home/vivek/catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
+ /* The debugging information file name */
+ char* dbgfname = "/home/vivek/catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
+ /* Wait for connection */
+ //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();
+ /* 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 */
+ buzz_script_step();
+ /* Cleanup */
+ // buzz_script_destroy();
+
+
+
+// %EndTag(ROS_OK)%
+ /**
+ * This is a message object. You stuff it with data, and then publish it.
+ */
+// %Tag(FILL_MESSAGE)%
+ std_msgs::String msg;
+
+ std::stringstream ss;
+ ss << "hello world " << count;
+ msg.data = ss.str();
+// %EndTag(FILL_MESSAGE)%
+
+// %Tag(ROSCONSOLE)%
+ ROS_INFO("%s", msg.data.c_str());
+// %EndTag(ROSCONSOLE)%
+
+ /**
+ * The publish() function is how you send messages. The parameter
+ * is the message object. The type of this object must agree with the type
+ * given as a template parameter to the advertise<>() call, as was done
+ * in the constructor above.
+ */
+
+// %Tag(PUBLISH)%
+ 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;
+}
+
+
+}
+// %EndTag(FULLTEXT)%
+