commit 537b1e2d91b515509eb502c86636ce7c13b56ece Author: Vivek Date: Mon Sep 19 14:08:14 2016 -0400 Ros_Buzz_Node diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..1a05e82 --- /dev/null +++ b/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.cpp + src/buzzuav_closures.cpp + src/uav_utility.cpp) +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/package.xml b/package.xml new file mode 100644 index 0000000..d47c4cc --- /dev/null +++ b/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/src/buzz_utility.cpp b/src/buzz_utility.cpp new file mode 100644 index 0000000..13f0f62 --- /dev/null +++ b/src/buzz_utility.cpp @@ -0,0 +1,576 @@ +#define _GNU_SOURCE +#include + +#include "buzz_utility.h" +#include "buzzuav_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 neighbour_location_handler(double distance, double azimuth, double elevation, int robot_id){ + buzzneighbors_reset(VM); + buzzneighbors_add(VM, robot_id, distance, azimuth, elevation); + // buzzinmsg_queue_append( + // VM->inmsgs, + // buzzmsg_payload_frombuffer(pl + tot, msgsz)); +/* Process messages */ + //buzzvm_process_inmsgs(VM); + +} +/****************************************/ + +/*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, buzzros_print)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_goto)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_takeoff)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_gohome)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_land)); + 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 */ + + /* 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 + */ + buzzuav_update_battery(VM); + buzzuav_update_prox(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/src/buzz_utility.h b/src/buzz_utility.h new file mode 100644 index 0000000..8df6404 --- /dev/null +++ b/src/buzz_utility.h @@ -0,0 +1,17 @@ +#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 neighbour_location_handler(double distance, double azimuth, double elevation, int robot_id); + +extern void buzz_script_step(); + +extern void buzz_script_destroy(); + +extern int buzz_script_done(); + +#endif diff --git a/src/buzzasm.h b/src/buzzasm.h new file mode 100644 index 0000000..69e71f6 --- /dev/null +++ b/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/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp new file mode 100644 index 0000000..e7499dd --- /dev/null +++ b/src/buzzuav_closures.cpp @@ -0,0 +1,160 @@ +#define _GNU_SOURCE +#include +#include "buzzuav_closures.h" +#include "uav_utility.h" +#include "mavros_msgs/CommandCode.h" + +// %Tag(ROS_HEADER)% +#include "ros/ros.h" +// %EndTag(ROS_HEADER)% + +double goto_pos[3]; +int cur_cmd; +/****************************************/ +/****************************************/ + +int buzzros_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]"); + ROS_INFO("BUZZ - [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) + ROS_INFO("BUZZ - [nil]"); + //fprintf(stdout, "[n-closure @%d]", o->c.value.ref); + else + ROS_INFO("BUZZ - [nil]"); + //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 buzzuav_goto(buzzvm_t vm) { + buzzvm_lnum_assert(vm, 3); + buzzvm_lload(vm, 1); /* Lattitude */ + buzzvm_lload(vm, 2); /* Longitude */ + buzzvm_lload(vm, 3); /* Altitude */ + buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); +goto_pos[0]=buzzvm_stack_at(vm, 1)->f.value * 10.0f; +goto_pos[1]=buzzvm_stack_at(vm, 2)->f.value * 10.0f; +goto_pos[2]=buzzvm_stack_at(vm, 3)->f.value * 10.0f; + return buzzvm_ret0(vm); +} + +double* getgoto(){ +return goto_pos; +} + +int getcmd(){ +return cur_cmd; +} + +/****************************************/ +/****************************************/ + +int buzzuav_takeoff(buzzvm_t vm) { + cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF; + return buzzvm_ret0(vm); +} + +int buzzuav_land(buzzvm_t vm) { + cur_cmd=mavros_msgs::CommandCode::NAV_LAND; + return buzzvm_ret0(vm); +} + +int buzzuav_gohome(buzzvm_t vm) { + cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; + return buzzvm_ret0(vm); +} + +/****************************************/ +float batt[3]; +void set_battery(float voltage,float current,float remaining){ +batt[0]=voltage; +batt[1]=current; +batt[2]=remaining; +} +/****************************************/ + +int buzzuav_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, batt[0]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "current", 1)); + buzzvm_pushf(vm, batt[1]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1)); + buzzvm_pushf(vm, batt[2]); + buzzvm_tput(vm); + buzzvm_gstore(vm); + return vm->state; +} + +/****************************************/ +/****************************************/ + +int buzzuav_update_prox(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/src/buzzuav_closures.h b/src/buzzuav_closures.h new file mode 100644 index 0000000..527b266 --- /dev/null +++ b/src/buzzuav_closures.h @@ -0,0 +1,53 @@ +#ifndef BUZZUAV_CLOSURES_H +#define BUZZUAV_CLOSURES_H + +#include + +/* + * prextern int() function in Buzz + */ +int buzzros_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 + */ +int buzzuav_goto(buzzvm_t vm); + +int getcmd(); +void set_battery(float voltage,float current,float remaining); +double* getgoto(); +/* + * 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 + */ +int buzzuav_takeoff(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 + */ +int buzzuav_land(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 + */ +int buzzuav_gohome(buzzvm_t vm); + +/* + * Updates battery information in Buzz + */ +int buzzuav_update_battery(buzzvm_t vm); + +/* + * Updates IR information in Buzz + * Proximity and ground sensors + */ +int buzzuav_update_prox(buzzvm_t vm); + +#endif diff --git a/src/out.basm b/src/out.basm new file mode 100644 index 0000000..50bca23 --- /dev/null +++ b/src/out.basm @@ -0,0 +1,36 @@ +!4 +'init +'step +'reset +'destroy + + pushs 0 + pushcn @__label_1 + gstore + pushs 1 + pushcn @__label_2 + gstore + pushs 2 + pushcn @__label_3 + gstore + pushs 3 + pushcn @__label_4 + gstore + nop + +@__label_0 + +@__exitpoint + done + +@__label_1 + ret0 |3,1,/home/vivek/BuzzKH4/build/test.bzz + +@__label_2 + ret0 |7,1,/home/vivek/BuzzKH4/build/test.bzz + +@__label_3 + ret0 |11,1,/home/vivek/BuzzKH4/build/test.bzz + +@__label_4 + ret0 |15,1,/home/vivek/BuzzKH4/build/test.bzz diff --git a/src/out.bdbg b/src/out.bdbg new file mode 100644 index 0000000..92d9b2b Binary files /dev/null and b/src/out.bdbg differ diff --git a/src/out.bo b/src/out.bo new file mode 100644 index 0000000..6c8c106 Binary files /dev/null and b/src/out.bo differ diff --git a/src/rosbuzz.cpp b/src/rosbuzz.cpp new file mode 100644 index 0000000..2ea18e2 --- /dev/null +++ b/src/rosbuzz.cpp @@ -0,0 +1,251 @@ +/* + + * Header + + */ +// %Tag(FULLTEXT)% +// %Tag(ROS_HEADER)% +#include "ros/ros.h" +// %EndTag(ROS_HEADER)% +#include "sensor_msgs/NavSatFix.h" +#include "mavros_msgs/GlobalPositionTarget.h" +#include "mavros_msgs/CommandCode.h" +#include "mavros_msgs/State.h" +#include "mavros_msgs/BatteryStatus.h" +// %Tag(MSG_HEADER)% +#include "std_msgs/String.h" +// %EndTag(MSG_HEADER)% +#include "std_msgs/Float64.h" +#include +#include +#include "buzzuav_closures.h" +#include "buzz_utility.h" +//extern "C" { +#include "uav_utility.h" +//} +#include +#include +#include +#include + +static int done = 0; + +static double cur_pos [3]; +double distance; +double azimuth; +double elevation; + +/** + * 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); +} +void set_cur_pos(double latitude, + double longitude, + double altitude){ +/* set the current position of the robot*/ +cur_pos [0] =latitude; +cur_pos [1] =longitude; +cur_pos [2] =altitude; +} + +void cvt_spherical_coordinates(double latitude, + double longitude, + double altitude){ +/** convert the current position coordination system from cartetion system (gps) to sperical system (Buzz) type **/ + distance = sqrt(pow(latitude,2.0)+pow(longitude,2.0)+pow(altitude,2.0)); + + elevation = atan(longitude/latitude); + azimuth = atan((sqrt(pow(latitude,2.0)+pow(longitude,2.0)))/altitude); +fprintf(stdout, "%.15f :distance value\n", distance); +fprintf(stdout, "%.15f :elevation\n", elevation); +fprintf(stdout, "%.15f :azimuth\n", azimuth); + +} + +// %Tag(CALLBACK)% +void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg) +{ +set_battery(msg->voltage,msg->current,msg->remaining); +} + +void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg) +{ + /*ROS_INFO("I heard current latitude: [%15f]", msg->latitude); + ROS_INFO("I heard current longitude: [%15f]", msg->longitude); + ROS_INFO("I heard current altitude: [%15f]", msg->altitude);*/ +/*obtain the current posituion of the robot*/ +//sperical* = cvt_spherical_coordinates(msg->latitude,msg->longitude,msg->altitude); +set_cur_pos(msg->latitude,msg->longitude,msg->altitude); +} +// %EndTag(CALLBACK)% + +// %Tag(CALLBACK)% +void neighbour_pos(const sensor_msgs::NavSatFix::ConstPtr& msg) +{ + +/*obtain the current position of the robot*/ + +double latitude =(msg->latitude-cur_pos[0]); +double longitude =(msg->longitude-cur_pos[1]); +double altitude =(msg->altitude-cur_pos[2]); + + ROS_INFO("I heard neighbour latitude: [%15f]", latitude); + ROS_INFO("I heard neighbour longitude: [%15f]", longitude); + ROS_INFO("I heard neighbour altitude: [%15f]", altitude); + +cvt_spherical_coordinates(latitude,longitude,altitude); +neighbour_location_handler( distance, azimuth, elevation, 01); + +} +// %EndTag(CALLBACK)% + + +static void ctrlc_handler(int sig) { + done = 1; +} + + + + +int main(int argc, char **argv) +{ + +// %Tag(INIT)% + ros::init(argc, argv, "rosBuzz"); +// %EndTag(INIT)% + +// %Tag(NODEHANDLE)% + ros::NodeHandle n_c; + // ros::NodeHandle n_n; +// %EndTag(NODEHANDLE)% + + +// %Tag(SUBSCRIBER)% + ros::Subscriber current_position = n_c.subscribe("current_pos", 1000, current_pos); + + ros::Subscriber neighbour_position = n_c.subscribe("neighbour_pos", 1000, neighbour_pos); + + ros::Subscriber battery_sub = n_c.subscribe("battery_state", 1000, battery); +// %EndTag(SUBSCRIBER)% + +// %Tag(PUBLISHER)% + ros::Publisher goto_pub = n_c.advertise("go_to", 1000); + + ros::Publisher cmds_pub = n_c.advertise("newstate", 1000); +// %EndTag(PUBLISHER)% + +// %Tag(LOOP_RATE)% + ros::Rate loop_rate(1); +// %EndTag(LOOP_RATE)% + + +// 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(); + + +// %Tag(FILL_MESSAGE)% + mavros_msgs::GlobalPositionTarget goto_set; + double* goto_pos = getgoto(); + + goto_set.latitude = goto_pos[0]; + goto_set.longitude=goto_pos[1]; + goto_set.altitude=goto_pos[2]; + + mavros_msgs::State cmds_set; +char tmp[20]; + sprintf(tmp,"%i",getcmd()); +cmds_set.mode = tmp; + +// %EndTag(FILL_MESSAGE)% + + +// %Tag(PUBLISH)% + goto_pub.publish(goto_set); + cmds_pub.publish(cmds_set); +// %EndTag(PUBLISH)% + + +// %Tag(SPINONCE)% + ros::spinOnce(); +// %EndTag(SPINONCE)% +// %Tag(RATE_SLEEP)% + loop_rate.sleep(); +// %EndTag(RATE_SLEEP)% + ++count; + } + /* Stop the robot */ + uav_done(); + +} + /* All done */ + return 0; + +} +// %EndTag(FULLTEXT)% + + diff --git a/src/test.bzz b/src/test.bzz new file mode 100644 index 0000000..18f692f --- /dev/null +++ b/src/test.bzz @@ -0,0 +1,15 @@ +# Executed once at init time. +function init() { +} + +# Executed at each time step. +function step() { +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/src/uav_utility.cpp b/src/uav_utility.cpp new file mode 100644 index 0000000..1dffbd9 --- /dev/null +++ b/src/uav_utility.cpp @@ -0,0 +1,55 @@ +#include "uav_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 uav_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 uav_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/src/uav_utility.h b/src/uav_utility.h new file mode 100644 index 0000000..b8aa940 --- /dev/null +++ b/src/uav_utility.h @@ -0,0 +1,12 @@ +#ifndef UAV_UTILITY_H +#define UAV_UTILITY_H + +//#include + +extern void uav_setup(); + +extern void uav_done(); + +//extern knet_dev_t* DSPIC; + +#endif