Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Ros_Buzz_Node

This commit is contained in:
Vivek 2016-09-19 14:08:14 -04:00
commit 537b1e2d91
14 changed files with 1480 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

199
CMakeLists.txt Normal file
View File

@ -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)

50
package.xml Normal file
View File

@ -0,0 +1,50 @@
<?xml version="1.0"?>
<package>
<name>rosbuzz</name>
<version>0.0.0</version>
<description>The rosbuzz package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="vivek@todo.todo">vivek</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/rosbuzz</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

576
src/buzz_utility.cpp Normal file
View File

@ -0,0 +1,576 @@
#define _GNU_SOURCE
#include <stdio.h>
#include "buzz_utility.h"
#include "buzzuav_closures.h"
#include <buzz/buzzdebug.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <errno.h>
#include <netdb.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <pthread.h>
/****************************************/
/****************************************/
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;
}
/****************************************/
/****************************************/

17
src/buzz_utility.h Normal file
View File

@ -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

56
src/buzzasm.h Normal file
View File

@ -0,0 +1,56 @@
#ifndef BUZZASM_H
#define BUZZASM_H
#include <buzz/buzzvm.h>
#include <buzz/buzzdebug.h>
#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

160
src/buzzuav_closures.cpp Normal file
View File

@ -0,0 +1,160 @@
#define _GNU_SOURCE
#include <stdio.h>
#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;
}
/****************************************/
/****************************************/

53
src/buzzuav_closures.h Normal file
View File

@ -0,0 +1,53 @@
#ifndef BUZZUAV_CLOSURES_H
#define BUZZUAV_CLOSURES_H
#include <buzz/buzzvm.h>
/*
* 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

36
src/out.basm Normal file
View File

@ -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

BIN
src/out.bdbg Normal file
View File

Binary file not shown.

BIN
src/out.bo Normal file
View File

Binary file not shown.

251
src/rosbuzz.cpp Normal file
View File

@ -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 <sstream>
#include <buzz/buzzasm.h>
#include "buzzuav_closures.h"
#include "buzz_utility.h"
//extern "C" {
#include "uav_utility.h"
//}
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <signal.h>
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 <stream> <msg_size> <file.bo> <file.bdb>\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<mavros_msgs::GlobalPositionTarget>("go_to", 1000);
ros::Publisher cmds_pub = n_c.advertise<mavros_msgs::State>("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)%

15
src/test.bzz Normal file
View File

@ -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() {
}

55
src/uav_utility.cpp Normal file
View File

@ -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");
}
/****************************************/
/****************************************/

12
src/uav_utility.h Normal file
View File

@ -0,0 +1,12 @@
#ifndef UAV_UTILITY_H
#define UAV_UTILITY_H
//#include <khepera/khepera.h>
extern void uav_setup();
extern void uav_done();
//extern knet_dev_t* DSPIC;
#endif