diff --git a/rosbuzz/CMakeLists.txt b/rosbuzz/CMakeLists.txt
deleted file mode 100644
index f70cd76..0000000
--- a/rosbuzz/CMakeLists.txt
+++ /dev/null
@@ -1,199 +0,0 @@
-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
deleted file mode 100644
index d47c4cc..0000000
--- a/rosbuzz/package.xml
+++ /dev/null
@@ -1,50 +0,0 @@
-
-
- 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
deleted file mode 100644
index 98e3eab..0000000
--- a/rosbuzz/src/buzz_utility.c
+++ /dev/null
@@ -1,557 +0,0 @@
-#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
deleted file mode 100644
index be3cb3d..0000000
--- a/rosbuzz/src/buzz_utility.h
+++ /dev/null
@@ -1,16 +0,0 @@
-#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
deleted file mode 100644
index 69e71f6..0000000
--- a/rosbuzz/src/buzzasm.h
+++ /dev/null
@@ -1,56 +0,0 @@
-#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
deleted file mode 100644
index 8d10c24..0000000
--- a/rosbuzz/src/buzzkh4_closures.c
+++ /dev/null
@@ -1,137 +0,0 @@
-#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
deleted file mode 100644
index f3ea2f4..0000000
--- a/rosbuzz/src/buzzkh4_closures.h
+++ /dev/null
@@ -1,36 +0,0 @@
-#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
deleted file mode 100644
index 0675fb6..0000000
--- a/rosbuzz/src/kh4_utility.c
+++ /dev/null
@@ -1,55 +0,0 @@
-#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
deleted file mode 100644
index 3ededb3..0000000
--- a/rosbuzz/src/kh4_utility.h
+++ /dev/null
@@ -1,12 +0,0 @@
-#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
deleted file mode 100644
index c73125d..0000000
--- a/rosbuzz/src/rosbuzz.cpp
+++ /dev/null
@@ -1,226 +0,0 @@
-/*
-
- * 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)%
-