From 632e7e49adba82fd07bf5fba9addd646c3d04a8b Mon Sep 17 00:00:00 2001 From: Vivek Date: Sat, 24 Sep 2016 18:27:11 -0400 Subject: [PATCH] repo clear --- rosbuzz/CMakeLists.txt | 199 ------------ rosbuzz/package.xml | 50 --- rosbuzz/src/buzz_utility.c | 557 --------------------------------- rosbuzz/src/buzz_utility.h | 16 - rosbuzz/src/buzzasm.h | 56 ---- rosbuzz/src/buzzkh4_closures.c | 137 -------- rosbuzz/src/buzzkh4_closures.h | 36 --- rosbuzz/src/kh4_utility.c | 55 ---- rosbuzz/src/kh4_utility.h | 12 - rosbuzz/src/rosbuzz.cpp | 226 ------------- 10 files changed, 1344 deletions(-) delete mode 100644 rosbuzz/CMakeLists.txt delete mode 100644 rosbuzz/package.xml delete mode 100644 rosbuzz/src/buzz_utility.c delete mode 100644 rosbuzz/src/buzz_utility.h delete mode 100644 rosbuzz/src/buzzasm.h delete mode 100644 rosbuzz/src/buzzkh4_closures.c delete mode 100644 rosbuzz/src/buzzkh4_closures.h delete mode 100644 rosbuzz/src/kh4_utility.c delete mode 100644 rosbuzz/src/kh4_utility.h delete mode 100644 rosbuzz/src/rosbuzz.cpp 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)% -