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

added simulation var in cmakelist and removed user vstig

This commit is contained in:
dave 2017-12-08 18:52:35 -05:00
parent 7e99692178
commit 72e51f3c82
9 changed files with 23 additions and 313 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
CMakeLists.txt
View File

@ -5,6 +5,11 @@ if(UNIX)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11")
endif()
if(SIM)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=1")
else()
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0")
endif()
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
roscpp

2
include/buzz_update.h
View File

@ -1,7 +1,7 @@
#ifndef BUZZ_UPDATE_H
#define BUZZ_UPDATE_H
/*Simulation or robot check*/
#define SIMULATION 1
//#define SIMULATION 1 // set in CMAKELIST
#include <stdlib.h>
#include <stdio.h>

5
include/buzz_utility.h
View File

@ -42,13 +42,8 @@ uint16_t* u64_cvt_u16(uint64_t u64);
int buzz_listen(const char* type,
int msg_size);
void add_user(int id, double latitude, double longitude, float altitude);
void update_users();
int make_table(buzzobj_t* t);
int buzzusers_add(int id, double latitude, double longitude, double altitude);
int buzzusers_reset();
int compute_users_rb();
int create_stig_tables();
void in_msg_append(uint64_t* payload);

3
include/buzzuav_closures.h
View File

@ -1,6 +1,5 @@
#pragma once
//#ifndef BUZZUAV_CLOSURES_H
//#define BUZZUAV_CLOSURES_H
#include <buzz/buzzvm.h>
#include <stdio.h>
#include "uav_utility.h"

1
include/roscontroller.h
View File

@ -212,7 +212,6 @@ private:
/*current position callback*/
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
void users_pos(const rosbuzz::neigh_pos msg);
/*current relative altitude callback*/

211
src/buzz_utility.cpp
View File

@ -1,8 +1,8 @@
/** @file buzz_utility.cpp
* @version 1.0
* @date 27.09.2016
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
* @author Vivek Shankar Varadharajan
* @brief Buzz Implementation as a node in ROS.
* @author Vivek Shankar Varadharajan and David St-Onge
* @copyright 2016 MistLab. All rights reserved.
*/
@ -17,119 +17,13 @@ namespace buzz_utility{
static char* BO_FNAME = 0;
static uint8_t* BO_BUF = 0;
static buzzdebug_t DBG_INFO = 0;
static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
static uint32_t MAX_MSG_SIZE = 250; // Maximum Msg size for sending update packets
static uint8_t Robot_id = 0;
static std::vector<uint8_t*> IN_MSG;
std::map< int, Pos_struct> users_map;
/****************************************/
void add_user(int id, double latitude, double longitude, float altitude)
{
Pos_struct pos_arr;
pos_arr.x=latitude;
pos_arr.y=longitude;
pos_arr.z=altitude;
map< int, buzz_utility::Pos_struct >::iterator it = users_map.find(id);
if(it!=users_map.end())
users_map.erase(it);
users_map.insert(make_pair(id, pos_arr));
//ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
}
void update_users(){
if(users_map.size()>0) {
// Reset users information
// buzzusers_reset();
// create_stig_tables();
// Get user id and update user information
map< int, Pos_struct >::iterator it;
for (it=users_map.begin(); it!=users_map.end(); ++it){
//ROS_INFO("Buzz_utility will save user %i.", it->first);
buzzusers_add(it->first,
(it->second).x,
(it->second).y,
(it->second).z);
}
}
}
/*int buzzusers_reset() {
if(VM->state != BUZZVM_STATE_READY) return VM->state;
//Make new table
buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
//make_table(&t);
if(VM->state != BUZZVM_STATE_READY) return VM->state;
//Register table as global symbol
//buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
buzzvm_push(VM, t);
buzzvm_gstore(VM);
//buzzvm_pushi(VM, 2);
//buzzvm_callc(VM);
return VM->state;
}*/
int buzzusers_add(int id, double latitude, double longitude, double altitude) {
if(VM->state != BUZZVM_STATE_READY) return VM->state;
// Get users "p" table
/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "get", 1));
buzzvm_tget(VM);*/
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_tget(VM);
if(buzzvm_stack_at(VM, 1)->o.type == BUZZTYPE_NIL) {
//ROS_INFO("Empty data, create a new table");
buzzvm_pop(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_pusht(VM);
buzzobj_t data = buzzvm_stack_at(VM, 1);
buzzvm_tput(VM);
buzzvm_push(VM, data);
}
// When we get here, the "data" table is on top of the stack
// Push user id
buzzvm_pushi(VM, id);
// Create entry table
buzzobj_t entry = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
// Insert latitude
buzzvm_push(VM, entry);
buzzvm_pushs(VM, buzzvm_string_register(VM, "la", 1));
buzzvm_pushf(VM, latitude);
buzzvm_tput(VM);
// Insert longitude
buzzvm_push(VM, entry);
buzzvm_pushs(VM, buzzvm_string_register(VM, "lo", 1));
buzzvm_pushf(VM, longitude);
buzzvm_tput(VM);
// Insert altitude
buzzvm_push(VM, entry);
buzzvm_pushs(VM, buzzvm_string_register(VM, "al", 1));
buzzvm_pushf(VM, altitude);
buzzvm_tput(VM);
// Save entry into data table
buzzvm_push(VM, entry);
buzzvm_tput(VM);
//ROS_INFO("Buzz_utility saved new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
// forcing the new table into the stigmergy....
/*buzzobj_t newt = buzzvm_stack_at(VM, 0);
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1));
buzzvm_push(VM, nbr);
buzzvm_pushi(VM, 2);
buzzvm_callc(VM);*/
//buzzvm_gstore(VM);
return VM->state;
}
/**************************************************************************/
/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */
/**************************************************************************/
@ -388,74 +282,6 @@ void in_message_process(){
return VM->state;
}
int create_stig_tables() {
/*
// usersvstig = stigmergy.create(123)
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
// get the stigmergy table from the global scope
buzzvm_pushs(VM, buzzvm_string_register(VM, "stigmergy", 1));
buzzvm_gload(VM);
// get the create method from the stigmergy table
buzzvm_pushs(VM, buzzvm_string_register(VM, "create", 1));
buzzvm_tget(VM);
// value of the stigmergy id
buzzvm_pushi(VM, 5);
// call the stigmergy.create() method
buzzvm_dump(VM);
// buzzvm_closure_call(VM, 1);
buzzvm_pushi(VM, 1);
buzzvm_callc(VM);
buzzvm_gstore(VM);
buzzvm_dump(VM);
//buzzusers_reset();
buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1));
buzzvm_push(VM, t);
buzzvm_dump(VM);
// buzzvm_closure_call(VM, 2);
buzzvm_pushi(VM, 2);
buzzvm_callc(VM);
//buzzvm_gstore(VM);
//buzzvm_dump(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "u", 1));
buzzvm_pusht(VM);
buzzvm_pushi(VM, 2);
buzzvm_call(VM, 0);
buzzvm_gstore(VM);*/
buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_push(VM,t);
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
buzzvm_pusht(VM);
buzzobj_t data = buzzvm_stack_at(VM, 1);
buzzvm_tput(VM);
buzzvm_push(VM, data);
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataL", 1));
buzzvm_pusht(VM);
data = buzzvm_stack_at(VM, 1);
buzzvm_tput(VM);
buzzvm_push(VM, data);
return VM->state;
}
/****************************************/
/*Sets the .bzz and .bdbg file*/
/****************************************/
@ -526,16 +352,6 @@ int create_stig_tables() {
buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
buzzvm_gstore(VM);
/* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] Error creating stigmergy tables", Robot_id);
//cout << "ERROR!!!! ---------- " << VM->errormsg << endl;
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
return 0;
}*/
// Execute the global part of the script
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
ROS_ERROR("Error executing global part, VM state : %i",VM->state);
@ -588,15 +404,6 @@ int create_stig_tables() {
buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
buzzvm_gstore(VM);
/* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] Error creating stigmergy tables", Robot_id);
//cout << "ERROR!!!! ---------- " << VM->errormsg << endl;
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
return 0;
}*/
// Execute the global part of the script
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
ROS_ERROR("Error executing global part, VM state : %i",VM->state);
@ -682,16 +489,6 @@ int create_stig_tables() {
buzz_error_info());
buzzvm_dump(VM);
}
/*Print swarm*/
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
//int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
//fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
/* Check swarm state -- Not crashing thanks to test added in check_swarm_members */
//int status = 1;
//buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
}
/****************************************/
@ -733,9 +530,7 @@ int create_stig_tables() {
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::update_neighbors(VM);
//update_users();
buzzuav_closures::buzzuav_update_flight_status(VM);
//set_robot_var(buzzdict_size(VM->swarmmembers)+1);
int a = buzzvm_function_call(VM, "step", 0);

48
src/buzzuav_closures.cpp
View File

@ -1,11 +1,11 @@
/** @file buzzuav_closures.cpp
* @version 1.0
* @date 27.09.2016
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
* @author Vivek Shankar Varadharajan
* @brief Buzz Implementation as a node in ROS.
* @author Vivek Shankar Varadharajan and David St-Onge
* @copyright 2016 MistLab. All rights reserved.
*/
//#define _GNU_SOURCE
#include "buzzuav_closures.h"
#include "math.h"
@ -192,11 +192,6 @@ int buzzros_print(buzzvm_t vm)
goto_pos[0]=dx;
goto_pos[1]=dy;
goto_pos[2]=height+dh;
/*double b = atan2(dy,dx); //bearing
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
gps_from_rb(d, b, goto_pos);
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
//printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
//ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], goto_pos[1], goto_pos[2]);
buzz_cmd = COMMAND_MOVETO; // TO DO what should we use
return buzzvm_ret0(vm);
@ -205,12 +200,8 @@ int buzzros_print(buzzvm_t vm)
int buzzuav_update_targets(buzzvm_t vm) {
if(vm->state != BUZZVM_STATE_READY) return vm->state;
buzzvm_pushs(vm, buzzvm_string_register(vm, "targets", 1));
//buzzobj_t t = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
//buzzvm_push(vm, t);
buzzvm_pusht(vm);
buzzobj_t targettbl = buzzvm_stack_at(vm, 1);
//buzzvm_tput(vm);
//buzzvm_dup(vm);
double rb[3], tmp[3];
map< int, buzz_utility::RB_struct >::iterator it;
for (it=targets_map.begin(); it!=targets_map.end(); ++it){
@ -736,31 +727,6 @@ int buzzros_print(buzzvm_t vm)
buzzvm_pushi(vm, 4);
buzzvm_push(vm, tProxRead);
buzzvm_tput(vm);
/*
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "bottom", 1));
buzzvm_pushf(vm, obst[0]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "front", 1));
buzzvm_pushf(vm, obst[1]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "right", 1));
buzzvm_pushf(vm, obst[2]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "back", 1));
buzzvm_pushf(vm, obst[3]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1));
buzzvm_pushf(vm, obst[4]);
buzzvm_tput(vm);
buzzvm_gstore(vm);*/
return vm->state;
}
@ -770,12 +736,4 @@ int buzzros_print(buzzvm_t vm)
int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);}
/***********************************************/
/* Store Ros controller object pointer */
/***********************************************/
//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin){
//roscontroller_ptr = roscontroller_ptrin;
//}
}

4
src/rosbuzz.cpp
View File

@ -1,8 +1,8 @@
/** @file rosbuzz.cpp
* @version 1.0
* @date 27.09.2016
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
* @author Vivek Shankar Varadharajan
* @brief Buzz Implementation as a node in ROS.
* @author Vivek Shankar Varadharajan and David St-Onge
* @copyright 2016 MistLab. All rights reserved.
*/

57
src/roscontroller.cpp
View File

@ -1,3 +1,11 @@
/** @file roscontroller.cpp
* @version 1.0
* @date 27.09.2016
* @brief Buzz Implementation as a node in ROS.
* @author Vivek Shankar Varadharajan and David St-Onge
* @copyright 2016 MistLab. All rights reserved.
*/
#include "roscontroller.h"
#include <thread>
namespace rosbzz_node {
@ -424,7 +432,6 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c)
capture_srv = n_c.serviceClient<mavros_msgs::CommandBool>(capture_srv_name);
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
users_sub = n_c.subscribe("users_pos", 5, &roscontroller::users_pos, this);
local_pos_sub = n_c.subscribe(local_pos_sub_name, 5, &roscontroller::local_pos_callback, this);
multi_msg = true;
@ -877,19 +884,6 @@ void roscontroller::local_pos_callback(
local_pos_new[2] = pose->pose.position.z;
}
void roscontroller::users_pos(const rosbuzz::neigh_pos data) {
int n = data.pos_neigh.size();
// ROS_INFO("Users array size: %i\n", n);
if (n > 0) {
for (int it = 0; it < n; ++it) {
buzz_utility::add_user(data.pos_neigh[it].position_covariance_type,
data.pos_neigh[it].latitude,
data.pos_neigh[it].longitude,
data.pos_neigh[it].altitude);
}
}
}
/*-------------------------------------------------------------
/ Update altitude into BVM from subscriber
/-------------------------------------------------------------*/
@ -1126,41 +1120,6 @@ void roscontroller::get_number_of_robots() {
no_cnt = 0;
}
}
/*
if(count_robots.current !=0){
std::map< int, int> count_count;
uint8_t index=0;
count_robots.history[count_robots.index]=neighbours_pos_map.size()+1;
//count_robots.current=neighbours_pos_map.size()+1;
count_robots.index++;
if(count_robots.index >9) count_robots.index =0;
for(int i=0;i<10;i++){
if(count_robots.history[i]==count_robots.current){
current_count++;
}
else{
if(count_robots.history[i]!=0){
odd_count++;
odd_val=count_robots.history[i];
}
}
}
if(odd_count>current_count){
count_robots.current=odd_val;
}
}
else{
if(neighbours_pos_map.size()!=0){
count_robots.history[count_robots.index]=neighbours_pos_map.size()+1;
//count_robots.current=neighbours_pos_map.size()+1;
count_robots.index++;
if(count_robots.index >9){
count_robots.index =0;
count_robots.current=neighbours_pos_map.size()+1;
}
}
}
*/
}
void roscontroller::get_xbee_status()