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

Initial commit for the buzz on ros as a node

This commit is contained in:
Vivek 2016-09-14 16:16:02 -04:00
commit 8923612405
10 changed files with 1356 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

199
rosbuzz/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.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)

50
rosbuzz/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>

554
rosbuzz/src/buzz_utility.c Normal file
View File

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

16
rosbuzz/src/buzz_utility.h Normal file
View File

@ -0,0 +1,16 @@
#ifndef BUZZ_UTILITY_H
#define BUZZ_UTILITY_H
extern int buzz_listen(const char* type,
int msg_size);
extern int buzz_script_set(const char* bo_filename,
const char* bdbg_filename);
extern void buzz_script_step();
extern void buzz_script_destroy();
extern int buzz_script_done();
#endif

56
rosbuzz/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

137
rosbuzz/src/buzzkh4_closures.c Normal file
View File

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

36
rosbuzz/src/buzzkh4_closures.h Normal file
View File

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

55
rosbuzz/src/kh4_utility.c Normal file
View File

@ -0,0 +1,55 @@
#include "kh4_utility.h"
#include "stdio.h"
/****************************************/
/****************************************/
//knet_dev_t* DSPIC;
static const int ACC_INC = 3;
static const int ACC_DIV = 0;
static const int MIN_SPEED_ACC = 20;
static const int MIN_SPEED_DEC = 1;
static const int MAX_SPEED = 400; /* mm/sec */
/****************************************/
/****************************************/
void kh4_setup() {
/* Set the libkhepera debug level */
/*kb_set_debug_level(2);
// initiate libkhepera and robot access
if(kh4_init(0, NULL) !=0)
{
printf("\nERROR: could not initialize the khepera!\n\n");
return;
}
/* open robot socket and store the handle in their respective pointers
DSPIC = knet_open("Khepera4:dsPic", KNET_BUS_I2C, 0, NULL);
/* Set speed profile
kh4_SetSpeedProfile(ACC_INC, /* Acceleration increment
ACC_DIV, /* Acceleration divider
MIN_SPEED_ACC, /* Minimum speed acc
MIN_SPEED_DEC, /* Minimum speed dec
MAX_SPEED, /* Maximum speed
DSPIC
);
kh4_SetMode(kh4RegSpeedProfile, DSPIC);
/* Mute ultrasonic sensor
kh4_activate_us(0, DSPIC);*/
}
/****************************************/
/****************************************/
void kh4_done() {
/* /* Stop wheels
kh4_set_speed(0, 0, DSPIC);
/* Set motors to idle
kh4_SetMode(kh4RegIdle, DSPIC);
/* Clear rgb leds because they consume energy
kh4_SetRGBLeds(0, 0, 0, 0, 0, 0, 0, 0, 0, DSPIC); */
fprintf(stdout, "Robot stopped.\n");
}
/****************************************/
/****************************************/

12
rosbuzz/src/kh4_utility.h Normal file
View File

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

241
rosbuzz/src/rosbuzz.cpp Normal file
View File

@ -0,0 +1,241 @@
/*
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
// %Tag(FULLTEXT)%
// %Tag(ROS_HEADER)%
#include "ros/ros.h"
// %EndTag(ROS_HEADER)%
// %Tag(MSG_HEADER)%
#include "std_msgs/String.h"
// %EndTag(MSG_HEADER)%
#include <sstream>
#include <buzz/buzzasm.h>
extern "C" {
#include "buzz_utility.h"
}
extern "C" {
#include "kh4_utility.h"
}
#include <stdlib.h>
#include <string.h>
#include <signal.h>
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 <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);
}
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, "talker");
// %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;
// %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 chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
// %EndTag(PUBLISHER)%
// %Tag(LOOP_RATE)%
ros::Rate loop_rate(10);
// %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)%
int count = 0;
while (ros::ok())
{
// 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 = argv[3];
/* The debugging information file name */
char* dbgfname = argv[4];
/* 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)) {
/* Main loop */
while(!done && !buzz_script_done())
buzz_script_step();
/* Cleanup */
buzz_script_destroy();
}
/* Stop the robot */
kh4_done();
/* All done */
return 0;
//
// Buzz stuff
// %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)%
chatter_pub.publish(msg);
// %EndTag(PUBLISH)%
// %Tag(SPINONCE)%
ros::spinOnce();
// %EndTag(SPINONCE)%
// %Tag(RATE_SLEEP)%
loop_rate.sleep();
// %EndTag(RATE_SLEEP)%
++count;
}
return 0;
}
// %EndTag(FULLTEXT)%