reformat comments
This commit is contained in:
dave
parent
d42d0a0cdb
commit
276d46ee83
|
|
@ -33,46 +33,95 @@ typedef enum {
|
|||
*/
|
||||
int buzzros_print(buzzvm_t vm);
|
||||
void setWPlist(std::string path);
|
||||
|
||||
/*
|
||||
* buzzuav_goto(latitude,longitude,altitude) function in Buzz
|
||||
* commands the UAV to go to a position supplied
|
||||
* closure to move following a vector
|
||||
*/
|
||||
int buzz_floor(buzzvm_t vm);
|
||||
int buzzuav_moveto(buzzvm_t vm);
|
||||
/*
|
||||
* closure to store a new GPS goal
|
||||
*/
|
||||
int buzzuav_storegoal(buzzvm_t vm);
|
||||
/*
|
||||
* closure to control the gimbal
|
||||
*/
|
||||
int buzzuav_setgimbal(buzzvm_t vm);
|
||||
/*
|
||||
* parse a csv list of waypoints
|
||||
*/
|
||||
void parse_gpslist();
|
||||
/*
|
||||
* closure to take a picture
|
||||
*/
|
||||
int buzzuav_takepicture(buzzvm_t vm);
|
||||
/* Returns the current command from local variable*/
|
||||
/*
|
||||
* Returns the current command from local variable
|
||||
*/
|
||||
int getcmd();
|
||||
/*Sets goto position from rc client*/
|
||||
/*
|
||||
* Sets goto position from rc client
|
||||
*/
|
||||
void rc_set_goto(int id, double latitude, double longitude, double altitude);
|
||||
/*Sets gimbal orientation from rc client*/
|
||||
/*
|
||||
*Sets gimbal orientation from rc client
|
||||
*/
|
||||
void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t);
|
||||
/*sets rc requested command */
|
||||
/*
|
||||
* sets rc requested command
|
||||
*/
|
||||
void rc_call(int rc_cmd);
|
||||
/* sets the battery state */
|
||||
/*
|
||||
* sets the battery state
|
||||
*/
|
||||
void set_battery(float voltage, float current, float remaining);
|
||||
/*
|
||||
* sets the xbee network status
|
||||
*/
|
||||
void set_deque_full(bool state);
|
||||
void set_rssi(float value);
|
||||
void set_raw_packet_loss(float value);
|
||||
void set_filtered_packet_loss(float value);
|
||||
void set_api_rssi(float value);
|
||||
/* sets current position */
|
||||
/*
|
||||
* sets current position
|
||||
*/
|
||||
void set_currentpos(double latitude, double longitude, double altitude);
|
||||
/*retuns the current go to position */
|
||||
/*
|
||||
* returns the current go to position
|
||||
*/
|
||||
double* getgoto();
|
||||
/*
|
||||
* returns the current Buzz state
|
||||
*/
|
||||
std::string getuavstate();
|
||||
/*
|
||||
* returns the gimbal commands
|
||||
*/
|
||||
float* getgimbal();
|
||||
/* updates flight status*/
|
||||
/*
|
||||
*updates flight status
|
||||
*/
|
||||
void flight_status_update(uint8_t state);
|
||||
/* Update neighbors table */
|
||||
/*
|
||||
*Update neighbors table
|
||||
*/
|
||||
void neighbour_pos_callback(int id, float range, float bearing, float elevation);
|
||||
/*
|
||||
* update neighbors from in msgs
|
||||
*/
|
||||
void update_neighbors(buzzvm_t vm);
|
||||
/*
|
||||
* closure to add a neighbor status
|
||||
*/
|
||||
int buzzuav_addNeiStatus(buzzvm_t vm);
|
||||
/*
|
||||
* returns the current array of neighbors status
|
||||
*/
|
||||
mavros_msgs::Mavlink get_status();
|
||||
|
||||
/*Flight status*/
|
||||
/*
|
||||
*Flight status
|
||||
*/
|
||||
void set_obstacle_dist(float dist[]);
|
||||
|
||||
/*
|
||||
|
|
@ -91,7 +140,8 @@ int buzzuav_disarm(buzzvm_t vm);
|
|||
*/
|
||||
int buzzuav_land(buzzvm_t vm);
|
||||
|
||||
/* Command the UAV to go to home location
|
||||
/*
|
||||
* Command the UAV to go to home location
|
||||
*/
|
||||
int buzzuav_gohome(buzzvm_t vm);
|
||||
|
||||
|
|
@ -107,7 +157,9 @@ int buzzuav_update_xbee_status(buzzvm_t vm);
|
|||
* Updates current position in Buzz
|
||||
*/
|
||||
int buzzuav_update_currentpos(buzzvm_t vm);
|
||||
int buzzuav_update_targets(buzzvm_t vm);
|
||||
/*
|
||||
* add new target in the BVM
|
||||
*/
|
||||
int buzzuav_addtargetRB(buzzvm_t vm);
|
||||
/*
|
||||
* Updates flight status and rc command in Buzz, put it in a tabel to acess it
|
||||
|
|
@ -121,10 +173,10 @@ int buzzuav_update_flight_status(buzzvm_t vm);
|
|||
* Proximity and ground sensors to do !!!!
|
||||
*/
|
||||
int buzzuav_update_prox(buzzvm_t vm);
|
||||
|
||||
/*
|
||||
* returns the current FC command
|
||||
*/
|
||||
int bzz_cmd();
|
||||
|
||||
int dummy_closure(buzzvm_t vm);
|
||||
|
||||
//#endif
|
||||
}
|
||||
|
|
|
|||
|
|
@ -99,4 +99,4 @@ References
|
|||
------
|
||||
* ROS and Buzz : consensus-based behaviors for heterogeneous teams. Submitted to the Internaional Conference on Robotics and Automation (September 2017). 6pgs. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843
|
||||
|
||||
* Over-The-Air Updates for Robotic Swarms. Accepted by IEEE Software (September 2017 - pending minor revision). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G.
|
||||
* Over-The-Air Updates for Robotic Swarms. Submitted to IEEE Software (August 2017). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G.
|
||||
|
|
|
|||
|
|
@ -24,10 +24,10 @@ std::map<int, Pos_struct> users_map;
|
|||
|
||||
/****************************************/
|
||||
|
||||
/**************************************************************************/
|
||||
/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */
|
||||
/**************************************************************************/
|
||||
uint16_t* u64_cvt_u16(uint64_t u64)
|
||||
/*
|
||||
/ Deserializes uint64_t into 4 uint16_t, freeing out is left to the user
|
||||
------------------------------------------------------------------------*/
|
||||
{
|
||||
uint16_t* out = new uint16_t[4];
|
||||
uint32_t int32_1 = u64 & 0xFFFFFFFF;
|
||||
|
|
@ -36,61 +36,66 @@ uint16_t* u64_cvt_u16(uint64_t u64)
|
|||
out[1] = (int32_1 & (0xFFFF0000)) >> 16;
|
||||
out[2] = int32_2 & 0xFFFF;
|
||||
out[3] = (int32_2 & (0xFFFF0000)) >> 16;
|
||||
// DEBUG
|
||||
// cout << " values " <<out[0] <<" "<<out[1] <<" "<<out[2] <<" "<<out[3] <<" ";
|
||||
return out;
|
||||
}
|
||||
|
||||
int get_robotid()
|
||||
/*
|
||||
/ return this robot ID
|
||||
------------------------------------------------------------------------*/
|
||||
{
|
||||
return Robot_id;
|
||||
}
|
||||
/***************************************************/
|
||||
/*Appends obtained messages to buzz in message Queue*/
|
||||
/***************************************************/
|
||||
|
||||
/*******************************************************************************************************************/
|
||||
/* Message format of payload (Each slot is uint64_t) */
|
||||
/* _______________________________________________________________________________________________________________ */
|
||||
/*| | |*/
|
||||
/*|Size in Uint64_t(but size is Uint16_t)|robot_id|Update msg size|Update msg|Update msgs+Buzz_msgs with size.....|*/
|
||||
/*|__________________________________________________________________________|____________________________________|*/
|
||||
/*******************************************************************************************************************/
|
||||
|
||||
void in_msg_append(uint64_t* payload)
|
||||
/*
|
||||
/ Appends obtained messages to buzz in message Queue
|
||||
---------------------------------------------------------------------
|
||||
/ Message format of payload (Each slot is uint64_t)
|
||||
/ _______________________________________________________________________________________________________________
|
||||
/| | |
|
||||
/|Size in Uint64_t(but size is Uint16_t)|robot_id|Update msg size|Update msg|Update msgs+Buzz_msgs with size.....|
|
||||
/|__________________________________________________________________________|____________________________________|
|
||||
---------------------------------------------------------------------------------------------------------------------*/
|
||||
{
|
||||
/* Go through messages and append them to the vector */
|
||||
// Go through messages and append them to the vector
|
||||
uint16_t* data = u64_cvt_u16((uint64_t)payload[0]);
|
||||
/*Size is at first 2 bytes*/
|
||||
// Size is at first 2 bytes
|
||||
uint16_t size = data[0] * sizeof(uint64_t);
|
||||
delete[] data;
|
||||
uint8_t* pl = (uint8_t*)malloc(size);
|
||||
/* Copy packet into temporary buffer */
|
||||
// Copy packet into temporary buffer
|
||||
memcpy(pl, payload, size);
|
||||
IN_MSG.push_back(pl);
|
||||
}
|
||||
|
||||
void in_message_process()
|
||||
/*
|
||||
/ Process msgs in
|
||||
---------------------------------------------------------------------------------------------------------------------*/
|
||||
{
|
||||
while (!IN_MSG.empty())
|
||||
{
|
||||
/* Go through messages and append them to the FIFO */
|
||||
// Go through messages and append them to the FIFO
|
||||
uint8_t* first_INmsg = (uint8_t*)IN_MSG.front();
|
||||
size_t tot = 0;
|
||||
/*Size is at first 2 bytes*/
|
||||
// Size is at first 2 bytes
|
||||
uint16_t size = (*(uint16_t*)first_INmsg) * sizeof(uint64_t);
|
||||
tot += sizeof(uint16_t);
|
||||
/*Decode neighbor Id*/
|
||||
// Decode neighbor Id
|
||||
uint16_t neigh_id = *(uint16_t*)(first_INmsg + tot);
|
||||
tot += sizeof(uint16_t);
|
||||
/* Go through the messages until there's nothing else to read */
|
||||
// Go through the messages until there's nothing else to read
|
||||
uint16_t unMsgSize = 0;
|
||||
/*Obtain Buzz messages push it into queue*/
|
||||
// Obtain Buzz messages push it into queue
|
||||
do
|
||||
{
|
||||
/* Get payload size */
|
||||
// Get payload size
|
||||
unMsgSize = *(uint16_t*)(first_INmsg + tot);
|
||||
tot += sizeof(uint16_t);
|
||||
/* Append message to the Buzz input message queue */
|
||||
// Append message to the Buzz input message queue
|
||||
if (unMsgSize > 0 && unMsgSize <= size - tot)
|
||||
{
|
||||
buzzinmsg_queue_append(VM, neigh_id, buzzmsg_payload_frombuffer(first_INmsg + tot, unMsgSize));
|
||||
|
|
@ -100,33 +105,34 @@ void in_message_process()
|
|||
free(first_INmsg);
|
||||
IN_MSG.erase(IN_MSG.begin());
|
||||
}
|
||||
/* Process messages VM call*/
|
||||
// Process messages VM call*
|
||||
buzzvm_process_inmsgs(VM);
|
||||
}
|
||||
|
||||
/***************************************************/
|
||||
/*Obtains messages from buzz out message Queue*/
|
||||
/***************************************************/
|
||||
uint64_t* obt_out_msg()
|
||||
/*
|
||||
/ Obtains messages from buzz out message Queue
|
||||
-------------------------------------------------*/
|
||||
{
|
||||
/* Process out messages */
|
||||
// Process out messages
|
||||
buzzvm_process_outmsgs(VM);
|
||||
uint8_t* buff_send = (uint8_t*)malloc(MAX_MSG_SIZE);
|
||||
memset(buff_send, 0, MAX_MSG_SIZE);
|
||||
/*Taking into consideration the sizes included at the end*/
|
||||
// Taking into consideration the sizes included at the end
|
||||
ssize_t tot = sizeof(uint16_t);
|
||||
/* Send robot id */
|
||||
// Send robot id
|
||||
*(uint16_t*)(buff_send + tot) = (uint16_t)VM->robot;
|
||||
tot += sizeof(uint16_t);
|
||||
/* Send messages from FIFO */
|
||||
// Send messages from FIFO
|
||||
do
|
||||
{
|
||||
/* Are there more messages? */
|
||||
// Are there more messages?
|
||||
if (buzzoutmsg_queue_isempty(VM))
|
||||
break;
|
||||
/* Get first message */
|
||||
// Get first message
|
||||
buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
|
||||
/* Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes */
|
||||
// Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes
|
||||
// DEBUG
|
||||
// ROS_INFO("read size : %i", (int)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)));
|
||||
if ((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MAX_MSG_SIZE)
|
||||
{
|
||||
|
|
@ -134,15 +140,15 @@ uint64_t* obt_out_msg()
|
|||
break;
|
||||
}
|
||||
|
||||
/* Add message length to data buffer */
|
||||
// Add message length to data buffer
|
||||
*(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m);
|
||||
tot += sizeof(uint16_t);
|
||||
|
||||
/* Add payload to data buffer */
|
||||
// Add payload to data buffer
|
||||
memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m));
|
||||
tot += buzzmsg_payload_size(m);
|
||||
|
||||
/* Get rid of message */
|
||||
// Get rid of message
|
||||
buzzoutmsg_queue_next(VM);
|
||||
buzzmsg_payload_destroy(&m);
|
||||
} while (1);
|
||||
|
|
@ -154,18 +160,18 @@ uint64_t* obt_out_msg()
|
|||
|
||||
memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t));
|
||||
free(buff_send);
|
||||
/*for(int i=0;i<total_size;i++){
|
||||
cout<<" payload from out msg "<<*(payload_64+i)<<endl;
|
||||
}*/
|
||||
/* Send message */
|
||||
// DEBUG
|
||||
// for(int i=0;i<total_size;i++){
|
||||
// cout<<" payload from out msg "<<*(payload_64+i)<<endl;
|
||||
//}
|
||||
// Send message
|
||||
return payload_64;
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
/*Buzz script not able to load*/
|
||||
/****************************************/
|
||||
|
||||
static const char* buzz_error_info()
|
||||
/*
|
||||
/ Buzz script not able to load
|
||||
---------------------------------*/
|
||||
{
|
||||
buzzdebug_entry_t dbg = *buzzdebug_info_get_fromoffset(DBG_INFO, &VM->pc);
|
||||
char* msg;
|
||||
|
|
@ -181,11 +187,10 @@ static const char* buzz_error_info()
|
|||
return msg;
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
/*Buzz hooks that can be used inside .bzz file*/
|
||||
/****************************************/
|
||||
|
||||
static int buzz_register_hooks()
|
||||
/*
|
||||
/ Buzz hooks that can be used inside .bzz file
|
||||
------------------------------------------------*/
|
||||
{
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
|
||||
|
|
@ -233,11 +238,10 @@ static int buzz_register_hooks()
|
|||
return VM->state;
|
||||
}
|
||||
|
||||
/**************************************************/
|
||||
/*Register dummy Buzz hooks for test during update*/
|
||||
/**************************************************/
|
||||
|
||||
static int testing_buzz_register_hooks()
|
||||
/*
|
||||
/ Register dummy Buzz hooks for test during update
|
||||
---------------------------------------------------*/
|
||||
{
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
|
||||
|
|
@ -285,14 +289,14 @@ static int testing_buzz_register_hooks()
|
|||
return VM->state;
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
/*Sets the .bzz and .bdbg file*/
|
||||
/****************************************/
|
||||
int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id)
|
||||
/*
|
||||
/ Sets the .bzz and .bdbg file
|
||||
---------------------------------*/
|
||||
{
|
||||
ROS_INFO(" Robot ID: %i", robot_id);
|
||||
Robot_id = robot_id;
|
||||
/* Read bytecode from file and fill the bo buffer*/
|
||||
// Read bytecode from file and fill the bo buffer
|
||||
FILE* fd = fopen(bo_filename, "rb");
|
||||
if (!fd)
|
||||
{
|
||||
|
|
@ -313,16 +317,16 @@ int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robo
|
|||
}
|
||||
fclose(fd);
|
||||
|
||||
/* Save bytecode file name */
|
||||
// Save bytecode file name
|
||||
BO_FNAME = strdup(bo_filename);
|
||||
|
||||
return buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
/*Sets a new update */
|
||||
/****************************************/
|
||||
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size)
|
||||
/*
|
||||
/ Sets a new update
|
||||
-----------------------*/
|
||||
{
|
||||
// Reset the Buzz VM
|
||||
if (VM)
|
||||
|
|
@ -379,10 +383,10 @@ int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_
|
|||
return 1;
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
/*Performs a initialization test */
|
||||
/****************************************/
|
||||
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size)
|
||||
/*
|
||||
/ Performs a initialization test
|
||||
-----------------------------------*/
|
||||
{
|
||||
// Reset the Buzz VM
|
||||
if (VM)
|
||||
|
|
@ -439,38 +443,40 @@ int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t
|
|||
return 1;
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
/*Swarm struct*/
|
||||
/****************************************/
|
||||
|
||||
struct buzzswarm_elem_s
|
||||
/*
|
||||
/ Swarm struct
|
||||
----------------*/
|
||||
{
|
||||
buzzdarray_t swarms;
|
||||
uint16_t age;
|
||||
};
|
||||
typedef struct buzzswarm_elem_s* buzzswarm_elem_t;
|
||||
|
||||
/*Step through the buzz script*/
|
||||
void update_sensors()
|
||||
/*
|
||||
/ Update from all external inputs
|
||||
-------------------------------*/
|
||||
{
|
||||
/* Update sensors*/
|
||||
// Update sensors
|
||||
buzzuav_closures::buzzuav_update_battery(VM);
|
||||
buzzuav_closures::buzzuav_update_xbee_status(VM);
|
||||
buzzuav_closures::buzzuav_update_prox(VM);
|
||||
buzzuav_closures::buzzuav_update_currentpos(VM);
|
||||
buzzuav_closures::update_neighbors(VM);
|
||||
buzzuav_closures::buzzuav_update_targets(VM);
|
||||
// update_users();
|
||||
buzzuav_closures::buzzuav_update_flight_status(VM);
|
||||
}
|
||||
|
||||
void buzz_script_step()
|
||||
/*
|
||||
/ Step through the buzz script
|
||||
-------------------------------*/
|
||||
{
|
||||
/*Process available messages*/
|
||||
// Process available messages
|
||||
in_message_process();
|
||||
/*Update sensors*/
|
||||
// Update sensors
|
||||
update_sensors();
|
||||
/* Call Buzz step() function */
|
||||
// Call Buzz step() function
|
||||
if (buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY)
|
||||
{
|
||||
ROS_ERROR("%s: execution terminated abnormally: %s", BO_FNAME, buzz_error_info());
|
||||
|
|
@ -478,11 +484,10 @@ void buzz_script_step()
|
|||
}
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
/*Destroy the bvm and other resorces*/
|
||||
/****************************************/
|
||||
|
||||
void buzz_script_destroy()
|
||||
/*
|
||||
/ Destroy the bvm and other related ressources
|
||||
-------------------------------------*/
|
||||
{
|
||||
if (VM)
|
||||
{
|
||||
|
|
@ -499,21 +504,20 @@ void buzz_script_destroy()
|
|||
ROS_INFO("Script execution stopped.");
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
/****************************************/
|
||||
|
||||
/****************************************/
|
||||
/*Execution completed*/
|
||||
/****************************************/
|
||||
|
||||
int buzz_script_done()
|
||||
/*
|
||||
/ Check if the BVM execution terminated
|
||||
---------------------------------------*/
|
||||
{
|
||||
return VM->state != BUZZVM_STATE_READY;
|
||||
}
|
||||
|
||||
int update_step_test()
|
||||
/*
|
||||
/ Step test for the update mechanism
|
||||
------------------------------------*/
|
||||
{
|
||||
/*Process available messages*/
|
||||
// Process available messages
|
||||
in_message_process();
|
||||
buzzuav_closures::buzzuav_update_battery(VM);
|
||||
buzzuav_closures::buzzuav_update_prox(VM);
|
||||
|
|
@ -533,11 +537,17 @@ int update_step_test()
|
|||
}
|
||||
|
||||
buzzvm_t get_vm()
|
||||
/*
|
||||
/ return the BVM
|
||||
----------------*/
|
||||
{
|
||||
return VM;
|
||||
}
|
||||
|
||||
void set_robot_var(int ROBOTS)
|
||||
/*
|
||||
/ set swarm size in the BVM
|
||||
-----------------------------*/
|
||||
{
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
|
||||
buzzvm_pushi(VM, ROBOTS);
|
||||
|
|
@ -545,6 +555,9 @@ void set_robot_var(int ROBOTS)
|
|||
}
|
||||
|
||||
int get_inmsg_size()
|
||||
/*
|
||||
/ get the incoming msgs size
|
||||
------------------------------*/
|
||||
{
|
||||
return IN_MSG.size();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -13,8 +13,6 @@ namespace buzzuav_closures
|
|||
{
|
||||
// TODO: Minimize the required global variables and put them in the header
|
||||
// static const rosbzz_node::roscontroller* roscontroller_ptr;
|
||||
/*forward declaration of ros controller ptr storing function*/
|
||||
// void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin);
|
||||
static double goto_pos[3];
|
||||
static double rc_goto_pos[3];
|
||||
static float rc_gimbal[4];
|
||||
|
|
@ -43,6 +41,9 @@ std::map<int, buzz_utility::neighbors_status> neighbors_status_map;
|
|||
/****************************************/
|
||||
|
||||
int buzzros_print(buzzvm_t vm)
|
||||
/*
|
||||
/ Buzz closure to print out
|
||||
----------------------------------------------------------- */
|
||||
{
|
||||
std::ostringstream buffer(std::ostringstream::ate);
|
||||
buffer << buzz_utility::get_robotid();
|
||||
|
|
@ -90,11 +91,17 @@ int buzzros_print(buzzvm_t vm)
|
|||
}
|
||||
|
||||
void setWPlist(string path)
|
||||
/*
|
||||
/ set the absolute path for a csv list of waypoints
|
||||
----------------------------------------------------------- */
|
||||
{
|
||||
WPlistname = path + "include/graphs/waypointlist.csv";
|
||||
}
|
||||
|
||||
float constrainAngle(float x)
|
||||
/*
|
||||
/ Wrap the angle between -pi, pi
|
||||
----------------------------------------------------------- */
|
||||
{
|
||||
x = fmod(x, 2 * M_PI);
|
||||
if (x < 0.0)
|
||||
|
|
@ -102,10 +109,10 @@ float constrainAngle(float x)
|
|||
return x;
|
||||
}
|
||||
|
||||
/*----------------------------------------/
|
||||
void rb_from_gps(double nei[], double out[], double cur[])
|
||||
/*
|
||||
/ Compute Range and Bearing from 2 GPS set of coordinates
|
||||
/----------------------------------------*/
|
||||
void rb_from_gps(double nei[], double out[], double cur[])
|
||||
{
|
||||
double d_lon = nei[1] - cur[1];
|
||||
double d_lat = nei[0] - cur[0];
|
||||
|
|
@ -117,6 +124,9 @@ void rb_from_gps(double nei[], double out[], double cur[])
|
|||
}
|
||||
|
||||
void parse_gpslist()
|
||||
/*
|
||||
/ parse a csv of GPS targets
|
||||
/----------------------------------------*/
|
||||
{
|
||||
// Open file:
|
||||
ROS_INFO("WP list file: %s", WPlistname.c_str());
|
||||
|
|
@ -147,6 +157,7 @@ void parse_gpslist()
|
|||
lat = atof(strtok(NULL, DELIMS));
|
||||
alt = atoi(strtok(NULL, DELIMS));
|
||||
tilt = atoi(strtok(NULL, DELIMS));
|
||||
// DEBUG
|
||||
// ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid);
|
||||
RB_arr.latitude = lat;
|
||||
RB_arr.longitude = lon;
|
||||
|
|
@ -164,15 +175,15 @@ void parse_gpslist()
|
|||
fin.close();
|
||||
}
|
||||
|
||||
/*----------------------------------------/
|
||||
int buzzuav_moveto(buzzvm_t vm)
|
||||
/*
|
||||
/ Buzz closure to move following a 2D vector
|
||||
/----------------------------------------*/
|
||||
int buzzuav_moveto(buzzvm_t vm)
|
||||
{
|
||||
buzzvm_lnum_assert(vm, 3);
|
||||
buzzvm_lload(vm, 1); /* dx */
|
||||
buzzvm_lload(vm, 2); /* dy */
|
||||
buzzvm_lload(vm, 3); /* dheight */
|
||||
buzzvm_lload(vm, 1); // dx
|
||||
buzzvm_lload(vm, 2); // dy
|
||||
buzzvm_lload(vm, 3); //* dheight
|
||||
buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
|
||||
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
|
||||
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
|
||||
|
|
@ -182,55 +193,17 @@ int buzzuav_moveto(buzzvm_t vm)
|
|||
goto_pos[0] = dx;
|
||||
goto_pos[1] = dy;
|
||||
goto_pos[2] = height + dh;
|
||||
// DEBUG
|
||||
// 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
|
||||
buzz_cmd = COMMAND_MOVETO; // TODO: standard mavros?
|
||||
return buzzvm_ret0(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));
|
||||
buzzvm_pusht(vm);
|
||||
buzzobj_t targettbl = buzzvm_stack_at(vm, 1);
|
||||
double rb[3], tmp[3];
|
||||
map<int, buzz_utility::RB_struct>::iterator it;
|
||||
for (it = targets_map.begin(); it != targets_map.end(); ++it)
|
||||
{
|
||||
tmp[0] = (it->second).latitude;
|
||||
tmp[1] = (it->second).longitude;
|
||||
tmp[2] = height;
|
||||
rb_from_gps(tmp, rb, cur_pos);
|
||||
// ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]);
|
||||
buzzvm_push(vm, targettbl);
|
||||
// When we get here, the "targets" table is on top of the stack
|
||||
// ROS_INFO("Buzz_utility will save user %i.", it->first);
|
||||
// Push user id
|
||||
buzzvm_pushi(vm, it->first);
|
||||
// Create entry table
|
||||
buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
|
||||
// Insert range
|
||||
buzzvm_push(vm, entry);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1));
|
||||
buzzvm_pushf(vm, rb[0]);
|
||||
buzzvm_tput(vm);
|
||||
// Insert longitude
|
||||
buzzvm_push(vm, entry);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1));
|
||||
buzzvm_pushf(vm, rb[1]);
|
||||
buzzvm_tput(vm);
|
||||
// Save entry into data table
|
||||
buzzvm_push(vm, entry);
|
||||
buzzvm_tput(vm);
|
||||
}
|
||||
buzzvm_gstore(vm);
|
||||
|
||||
return vm->state;
|
||||
}
|
||||
|
||||
int buzzuav_addtargetRB(buzzvm_t vm)
|
||||
/*
|
||||
/ Buzz closure to add a target (goal) GPS
|
||||
/----------------------------------------*/
|
||||
{
|
||||
buzzvm_lnum_assert(vm, 3);
|
||||
buzzvm_lload(vm, 1); // longitude
|
||||
|
|
@ -249,7 +222,6 @@ int buzzuav_addtargetRB(buzzvm_t vm)
|
|||
rb_from_gps(tmp, rb, cur_pos);
|
||||
if (fabs(rb[0]) < 100.0)
|
||||
{
|
||||
// printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
|
||||
buzz_utility::RB_struct RB_arr;
|
||||
RB_arr.latitude = tmp[0];
|
||||
RB_arr.longitude = tmp[1];
|
||||
|
|
@ -260,6 +232,7 @@ int buzzuav_addtargetRB(buzzvm_t vm)
|
|||
if (it != targets_map.end())
|
||||
targets_map.erase(it);
|
||||
targets_map.insert(make_pair(uid, RB_arr));
|
||||
// DEBUG
|
||||
// ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
|
||||
return vm->state;
|
||||
}
|
||||
|
|
@ -270,6 +243,9 @@ int buzzuav_addtargetRB(buzzvm_t vm)
|
|||
}
|
||||
|
||||
int buzzuav_addNeiStatus(buzzvm_t vm)
|
||||
/*
|
||||
/ closure to add neighbors status to the BVM
|
||||
/----------------------------------------*/
|
||||
{
|
||||
buzzvm_lnum_assert(vm, 5);
|
||||
buzzvm_lload(vm, 1); // fc
|
||||
|
|
@ -296,6 +272,9 @@ int buzzuav_addNeiStatus(buzzvm_t vm)
|
|||
}
|
||||
|
||||
mavros_msgs::Mavlink get_status()
|
||||
/*
|
||||
/ return neighbors status from BVM
|
||||
/----------------------------------------*/
|
||||
{
|
||||
mavros_msgs::Mavlink payload_out;
|
||||
map<int, buzz_utility::neighbors_status>::iterator it;
|
||||
|
|
@ -307,27 +286,25 @@ mavros_msgs::Mavlink get_status()
|
|||
payload_out.payload64.push_back(it->second.xbee);
|
||||
payload_out.payload64.push_back(it->second.flight_status);
|
||||
}
|
||||
/*Add Robot id and message number to the published message*/
|
||||
// Add Robot id and message number to the published message
|
||||
payload_out.sysid = (uint8_t)neighbors_status_map.size();
|
||||
/*payload_out.msgid = (uint32_t)message_number;
|
||||
|
||||
message_number++;*/
|
||||
return payload_out;
|
||||
}
|
||||
/*----------------------------------------/
|
||||
|
||||
int buzzuav_takepicture(buzzvm_t vm)
|
||||
/*
|
||||
/ Buzz closure to take a picture here.
|
||||
/----------------------------------------*/
|
||||
int buzzuav_takepicture(buzzvm_t vm)
|
||||
{
|
||||
// cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST;
|
||||
buzz_cmd = COMMAND_PICTURE;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
/*----------------------------------------/
|
||||
int buzzuav_setgimbal(buzzvm_t vm)
|
||||
/*
|
||||
/ Buzz closure to change locally the gimbal orientation
|
||||
/----------------------------------------*/
|
||||
int buzzuav_setgimbal(buzzvm_t vm)
|
||||
{
|
||||
buzzvm_lnum_assert(vm, 4);
|
||||
buzzvm_lload(vm, 1); // time
|
||||
|
|
@ -343,15 +320,15 @@ int buzzuav_setgimbal(buzzvm_t vm)
|
|||
rc_gimbal[2] = buzzvm_stack_at(vm, 2)->f.value;
|
||||
rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value;
|
||||
|
||||
ROS_WARN("Set RC_GIMBAL ---- %f %f %f (%f)", rc_gimbal[0], rc_gimbal[1], rc_gimbal[2], rc_gimbal[3]);
|
||||
ROS_INFO("Set RC_GIMBAL ---- %f %f %f (%f)", rc_gimbal[0], rc_gimbal[1], rc_gimbal[2], rc_gimbal[3]);
|
||||
buzz_cmd = COMMAND_GIMBAL;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
/*----------------------------------------/
|
||||
int buzzuav_storegoal(buzzvm_t vm)
|
||||
/*
|
||||
/ Buzz closure to store locally a GPS destination from the fleet
|
||||
/----------------------------------------*/
|
||||
int buzzuav_storegoal(buzzvm_t vm)
|
||||
{
|
||||
buzzvm_lnum_assert(vm, 3);
|
||||
buzzvm_lload(vm, 1); // altitude
|
||||
|
|
@ -380,21 +357,25 @@ int buzzuav_storegoal(buzzvm_t vm)
|
|||
if (fabs(rb[0]) < 150.0)
|
||||
rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]);
|
||||
|
||||
ROS_WARN("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
|
||||
ROS_INFO("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
/*----------------------------------------/
|
||||
/ Buzz closure to arm/disarm the drone, useful for field tests to ensure all systems are up and running
|
||||
/----------------------------------------*/
|
||||
int buzzuav_arm(buzzvm_t vm)
|
||||
/*
|
||||
/ Buzz closure to arm
|
||||
/---------------------------------------*/
|
||||
{
|
||||
cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
|
||||
printf(" Buzz requested Arm \n");
|
||||
buzz_cmd = COMMAND_ARM;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
int buzzuav_disarm(buzzvm_t vm)
|
||||
/*
|
||||
/ Buzz closure to disarm
|
||||
/---------------------------------------*/
|
||||
{
|
||||
cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
|
||||
printf(" Buzz requested Disarm \n");
|
||||
|
|
@ -402,10 +383,10 @@ int buzzuav_disarm(buzzvm_t vm)
|
|||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
/*---------------------------------------/
|
||||
/ Buzz closure for basic UAV commands
|
||||
/---------------------------------------*/
|
||||
int buzzuav_takeoff(buzzvm_t vm)
|
||||
/*
|
||||
/ Buzz closure to takeoff
|
||||
/---------------------------------------*/
|
||||
{
|
||||
buzzvm_lnum_assert(vm, 1);
|
||||
buzzvm_lload(vm, 1); /* Altitude */
|
||||
|
|
@ -419,6 +400,9 @@ int buzzuav_takeoff(buzzvm_t vm)
|
|||
}
|
||||
|
||||
int buzzuav_land(buzzvm_t vm)
|
||||
/*
|
||||
/ Buzz closure to land
|
||||
/-------------------------------------------------------------*/
|
||||
{
|
||||
cur_cmd = mavros_msgs::CommandCode::NAV_LAND;
|
||||
printf(" Buzz requested Land !!! \n");
|
||||
|
|
@ -427,6 +411,9 @@ int buzzuav_land(buzzvm_t vm)
|
|||
}
|
||||
|
||||
int buzzuav_gohome(buzzvm_t vm)
|
||||
/*
|
||||
/ Buzz closure to return Home
|
||||
/-------------------------------------------------------------*/
|
||||
{
|
||||
cur_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
|
||||
printf(" Buzz requested gohome !!! \n");
|
||||
|
|
@ -434,20 +421,26 @@ int buzzuav_gohome(buzzvm_t vm)
|
|||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
/*-------------------------------
|
||||
/ Get/Set to transfer variable from Roscontroller to buzzuav
|
||||
/------------------------------------*/
|
||||
double* getgoto()
|
||||
/*
|
||||
/ return the GPS goal
|
||||
/-------------------------------------------------------------*/
|
||||
{
|
||||
return goto_pos;
|
||||
}
|
||||
|
||||
float* getgimbal()
|
||||
/*
|
||||
/ return current gimbal commands
|
||||
---------------------------------------*/
|
||||
{
|
||||
return rc_gimbal;
|
||||
}
|
||||
|
||||
string getuavstate()
|
||||
/*
|
||||
/ return current BVM state
|
||||
---------------------------------------*/
|
||||
{
|
||||
static buzzvm_t VM = buzz_utility::get_vm();
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
|
||||
|
|
@ -458,11 +451,17 @@ string getuavstate()
|
|||
}
|
||||
|
||||
int getcmd()
|
||||
/*
|
||||
/ return current mavros command to the FC
|
||||
---------------------------------------*/
|
||||
{
|
||||
return cur_cmd;
|
||||
}
|
||||
|
||||
int bzz_cmd()
|
||||
/*
|
||||
/ return and clean the custom command from Buzz to the FC
|
||||
----------------------------------------------------------*/
|
||||
{
|
||||
int cmd = buzz_cmd;
|
||||
buzz_cmd = 0;
|
||||
|
|
@ -470,6 +469,9 @@ int bzz_cmd()
|
|||
}
|
||||
|
||||
void rc_set_goto(int id, double latitude, double longitude, double altitude)
|
||||
/*
|
||||
/ update interface RC GPS goal input
|
||||
-----------------------------------*/
|
||||
{
|
||||
rc_id = id;
|
||||
rc_goto_pos[0] = latitude;
|
||||
|
|
@ -478,6 +480,9 @@ void rc_set_goto(int id, double latitude, double longitude, double altitude)
|
|||
}
|
||||
|
||||
void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t)
|
||||
/*
|
||||
/ update interface RC gimbal control input
|
||||
-----------------------------------*/
|
||||
{
|
||||
rc_id = id;
|
||||
rc_gimbal[0] = yaw;
|
||||
|
|
@ -487,17 +492,26 @@ void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t)
|
|||
}
|
||||
|
||||
void rc_call(int rc_cmd_in)
|
||||
/*
|
||||
/ update interface RC command input
|
||||
-----------------------------------*/
|
||||
{
|
||||
rc_cmd = rc_cmd_in;
|
||||
}
|
||||
|
||||
void set_obstacle_dist(float dist[])
|
||||
/*
|
||||
/ update interface proximity value array
|
||||
-----------------------------------*/
|
||||
{
|
||||
for (int i = 0; i < 5; i++)
|
||||
obst[i] = dist[i];
|
||||
}
|
||||
|
||||
void set_battery(float voltage, float current, float remaining)
|
||||
/*
|
||||
/ update interface battery value array
|
||||
-----------------------------------*/
|
||||
{
|
||||
batt[0] = voltage;
|
||||
batt[1] = current;
|
||||
|
|
@ -505,6 +519,9 @@ void set_battery(float voltage, float current, float remaining)
|
|||
}
|
||||
|
||||
int buzzuav_update_battery(buzzvm_t vm)
|
||||
/*
|
||||
/ update BVM battery table
|
||||
-----------------------------------*/
|
||||
{
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1));
|
||||
buzzvm_pusht(vm);
|
||||
|
|
@ -524,6 +541,9 @@ int buzzuav_update_battery(buzzvm_t vm)
|
|||
return vm->state;
|
||||
}
|
||||
|
||||
/*
|
||||
/ Set of function to update interface variable of xbee network status
|
||||
----------------------------------------------------------------------*/
|
||||
void set_deque_full(bool state)
|
||||
{
|
||||
deque_full = state;
|
||||
|
|
@ -550,6 +570,9 @@ void set_api_rssi(float value)
|
|||
}
|
||||
|
||||
int buzzuav_update_xbee_status(buzzvm_t vm)
|
||||
/*
|
||||
/ update BVM xbee_status table
|
||||
-----------------------------------*/
|
||||
{
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "xbee_status", 1));
|
||||
buzzvm_pusht(vm);
|
||||
|
|
@ -577,16 +600,16 @@ int buzzuav_update_xbee_status(buzzvm_t vm)
|
|||
return vm->state;
|
||||
}
|
||||
|
||||
/***************************************/
|
||||
/*current pos update*/
|
||||
/***************************************/
|
||||
void set_currentpos(double latitude, double longitude, double altitude)
|
||||
/*
|
||||
/ update interface position array
|
||||
-----------------------------------*/
|
||||
{
|
||||
cur_pos[0] = latitude;
|
||||
cur_pos[1] = longitude;
|
||||
cur_pos[2] = altitude;
|
||||
}
|
||||
/*adds neighbours position*/
|
||||
// adds neighbours position
|
||||
void neighbour_pos_callback(int id, float range, float bearing, float elevation)
|
||||
{
|
||||
buzz_utility::Pos_struct pos_arr;
|
||||
|
|
@ -599,12 +622,12 @@ void neighbour_pos_callback(int id, float range, float bearing, float elevation)
|
|||
neighbors_map.insert(make_pair(id, pos_arr));
|
||||
}
|
||||
|
||||
/* update at each step the VM table */
|
||||
// update at each step the VM table
|
||||
void update_neighbors(buzzvm_t vm)
|
||||
{
|
||||
/* Reset neighbor information */
|
||||
// Reset neighbor information
|
||||
buzzneighbors_reset(vm);
|
||||
/* Get robot id and update neighbor information */
|
||||
// Get robot id and update neighbor information
|
||||
map<int, buzz_utility::Pos_struct>::iterator it;
|
||||
for (it = neighbors_map.begin(); it != neighbors_map.end(); ++it)
|
||||
{
|
||||
|
|
@ -612,8 +635,10 @@ void update_neighbors(buzzvm_t vm)
|
|||
}
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
int buzzuav_update_currentpos(buzzvm_t vm)
|
||||
/*
|
||||
/ Update the BVM position table
|
||||
/------------------------------------------------------*/
|
||||
{
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
|
||||
buzzvm_pusht(vm);
|
||||
|
|
@ -634,16 +659,18 @@ int buzzuav_update_currentpos(buzzvm_t vm)
|
|||
}
|
||||
|
||||
void flight_status_update(uint8_t state)
|
||||
/*
|
||||
/ Update the interface status variable
|
||||
/------------------------------------------------------*/
|
||||
{
|
||||
status = state;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------
|
||||
int buzzuav_update_flight_status(buzzvm_t vm)
|
||||
/*
|
||||
/ Create the generic robot table with status, remote controller current comand and destination
|
||||
/ and current position of the robot
|
||||
/ TODO: change global name for robot
|
||||
/------------------------------------------------------*/
|
||||
int buzzuav_update_flight_status(buzzvm_t vm)
|
||||
{
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1));
|
||||
buzzvm_pusht(vm);
|
||||
|
|
@ -657,7 +684,6 @@ int buzzuav_update_flight_status(buzzvm_t vm)
|
|||
buzzvm_pushi(vm, status);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_gstore(vm);
|
||||
// also set rc_controllers goto
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1));
|
||||
buzzvm_pusht(vm);
|
||||
buzzvm_dup(vm);
|
||||
|
|
@ -680,32 +706,31 @@ int buzzuav_update_flight_status(buzzvm_t vm)
|
|||
return vm->state;
|
||||
}
|
||||
|
||||
/******************************************************/
|
||||
/*Create an obstacle Buzz table from proximity sensors*/
|
||||
/* Acessing proximity in buzz script
|
||||
proximity[0].angle and proximity[0].value - front
|
||||
"" "" "" - right and back
|
||||
proximity[3].angle and proximity[3].value - left
|
||||
proximity[4].angle = -1 and proximity[4].value -bottom */
|
||||
/******************************************************/
|
||||
|
||||
int buzzuav_update_prox(buzzvm_t vm)
|
||||
/*
|
||||
/ Create an obstacle Buzz table from proximity sensors
|
||||
/ Acessing proximity in buzz script
|
||||
/ proximity[0].angle and proximity[0].value - front
|
||||
/ "" "" "" - right and back
|
||||
/ proximity[3].angle and proximity[3].value - left
|
||||
/ proximity[4].angle = -1 and proximity[4].value -bottom
|
||||
-------------------------------------------------------------*/
|
||||
{
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
|
||||
buzzvm_pusht(vm);
|
||||
buzzobj_t tProxTable = buzzvm_stack_at(vm, 1);
|
||||
buzzvm_gstore(vm);
|
||||
|
||||
/* Fill into the proximity table */
|
||||
// Fill into the proximity table
|
||||
buzzobj_t tProxRead;
|
||||
float angle = 0;
|
||||
for (size_t i = 0; i < 4; ++i)
|
||||
{
|
||||
/* Create table for i-th read */
|
||||
// Create table for i-th read
|
||||
buzzvm_pusht(vm);
|
||||
tProxRead = buzzvm_stack_at(vm, 1);
|
||||
buzzvm_pop(vm);
|
||||
/* Fill in the read */
|
||||
// Fill in the read
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
|
||||
buzzvm_pushf(vm, obst[i + 1]);
|
||||
|
|
@ -714,19 +739,19 @@ int buzzuav_update_prox(buzzvm_t vm)
|
|||
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
|
||||
buzzvm_pushf(vm, angle);
|
||||
buzzvm_tput(vm);
|
||||
/* Store read table in the proximity table */
|
||||
// Store read table in the proximity table
|
||||
buzzvm_push(vm, tProxTable);
|
||||
buzzvm_pushi(vm, i);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_tput(vm);
|
||||
angle += 1.5708;
|
||||
}
|
||||
/* Create table for bottom read */
|
||||
// Create table for bottom read
|
||||
angle = -1;
|
||||
buzzvm_pusht(vm);
|
||||
tProxRead = buzzvm_stack_at(vm, 1);
|
||||
buzzvm_pop(vm);
|
||||
/* Fill in the read */
|
||||
// Fill in the read
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
|
||||
buzzvm_pushf(vm, obst[0]);
|
||||
|
|
@ -735,7 +760,7 @@ int buzzuav_update_prox(buzzvm_t vm)
|
|||
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
|
||||
buzzvm_pushf(vm, angle);
|
||||
buzzvm_tput(vm);
|
||||
/*Store read table in the proximity table*/
|
||||
// Store read table in the proximity table
|
||||
buzzvm_push(vm, tProxTable);
|
||||
buzzvm_pushi(vm, 4);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
|
|
@ -743,11 +768,10 @@ int buzzuav_update_prox(buzzvm_t vm)
|
|||
return vm->state;
|
||||
}
|
||||
|
||||
/**********************************************/
|
||||
/*Dummy closure for use during update testing */
|
||||
/**********************************************/
|
||||
|
||||
int dummy_closure(buzzvm_t vm)
|
||||
/*
|
||||
/ Dummy closure for use during update testing
|
||||
----------------------------------------------------*/
|
||||
{
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -8,19 +8,17 @@
|
|||
|
||||
#include "roscontroller.h"
|
||||
|
||||
/**
|
||||
* This program implements Buzz node in ros using mavros_msgs.
|
||||
*/
|
||||
|
||||
int main(int argc, char** argv)
|
||||
/*
|
||||
/ This program implements Buzz in a ROS node using mavros_msgs.
|
||||
-----------------------------------------------------------------*/
|
||||
{
|
||||
/*Initialize rosBuzz node*/
|
||||
// Initialize rosBuzz node
|
||||
ros::init(argc, argv, "rosBuzz");
|
||||
ros::NodeHandle nh_priv("~");
|
||||
ros::NodeHandle nh;
|
||||
rosbzz_node::roscontroller RosController(nh, nh_priv);
|
||||
/*Register ros controller object inside buzz*/
|
||||
// buzzuav_closures::set_ros_controller_ptr(&RosController);
|
||||
|
||||
RosController.RosControllerRun();
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue