Class Implementaion and code clean commit
This commit is contained in:
Vivek Shankar Varadharajan
parent
6bb28d7ae6
commit
9b25ad53bd
|
|
@ -1,5 +1,10 @@
|
||||||
cmake_minimum_required(VERSION 2.8.3)
|
cmake_minimum_required(VERSION 2.8.3)
|
||||||
project(rosbuzz)
|
project(rosbuzz)
|
||||||
|
|
||||||
|
if(UNIX)
|
||||||
|
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11")
|
||||||
|
endif()
|
||||||
|
|
||||||
## Find catkin macros and libraries
|
## Find catkin macros and libraries
|
||||||
find_package(catkin REQUIRED COMPONENTS
|
find_package(catkin REQUIRED COMPONENTS
|
||||||
roscpp
|
roscpp
|
||||||
|
|
@ -13,18 +18,24 @@ find_package(catkin REQUIRED COMPONENTS
|
||||||
###################################
|
###################################
|
||||||
|
|
||||||
catkin_package(
|
catkin_package(
|
||||||
#CATKIN_DEPENDS message_runtime
|
INCLUDE_DIRS include
|
||||||
|
# LIBRARIES xbee_ros_node
|
||||||
|
CATKIN_DEPENDS roscpp std_msgs mavros_msgs sensor_msgs
|
||||||
|
# DEPENDS system_lib
|
||||||
)
|
)
|
||||||
|
|
||||||
###########
|
###########
|
||||||
## Build ##
|
## Build ##
|
||||||
###########
|
###########
|
||||||
|
|
||||||
include_directories(
|
include_directories(
|
||||||
|
include ${rosbuzz_INCLUDE_DIRS}
|
||||||
${catkin_INCLUDE_DIRS}
|
${catkin_INCLUDE_DIRS}
|
||||||
)
|
)
|
||||||
|
|
||||||
# Executables
|
# Executables
|
||||||
add_executable(rosbuzz_node src/rosbuzz.cpp
|
add_executable(rosbuzz_node src/rosbuzz.cpp
|
||||||
|
src/roscontroller.cpp
|
||||||
src/buzz_utility.cpp
|
src/buzz_utility.cpp
|
||||||
src/buzzuav_closures.cpp
|
src/buzzuav_closures.cpp
|
||||||
src/uav_utility.cpp)
|
src/uav_utility.cpp)
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,44 @@
|
||||||
|
#pragma once
|
||||||
|
#include <stdio.h>
|
||||||
|
#include "buzz_utility.h"
|
||||||
|
#include "buzzuav_closures.h"
|
||||||
|
#include <buzz/buzzdebug.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <iostream>
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <map>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
namespace buzz_utility{
|
||||||
|
|
||||||
|
struct pos_struct
|
||||||
|
{
|
||||||
|
double x,y,z;
|
||||||
|
pos_struct(double x,double y,double z):x(x),y(y),z(z){};
|
||||||
|
pos_struct(){}
|
||||||
|
};
|
||||||
|
|
||||||
|
typedef struct pos_struct Pos_struct ;
|
||||||
|
|
||||||
|
uint16_t* u64_cvt_u16(uint64_t u64);
|
||||||
|
|
||||||
|
int buzz_listen(const char* type,
|
||||||
|
int msg_size);
|
||||||
|
|
||||||
|
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map);
|
||||||
|
|
||||||
|
void in_msg_process(uint64_t* payload);
|
||||||
|
|
||||||
|
uint64_t* out_msg_process();
|
||||||
|
|
||||||
|
int buzz_script_set(const char* bo_filename,
|
||||||
|
const char* bdbg_filename, int robot_id);
|
||||||
|
void buzz_script_step();
|
||||||
|
|
||||||
|
void buzz_script_destroy();
|
||||||
|
|
||||||
|
int buzz_script_done();
|
||||||
|
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,58 @@
|
||||||
|
#pragma once
|
||||||
|
//#ifndef BUZZUAV_CLOSURES_H
|
||||||
|
//#define BUZZUAV_CLOSURES_H
|
||||||
|
#include <buzz/buzzvm.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include "uav_utility.h"
|
||||||
|
#include "mavros_msgs/CommandCode.h"
|
||||||
|
#include "ros/ros.h"
|
||||||
|
|
||||||
|
namespace buzzuav_closures{
|
||||||
|
/*
|
||||||
|
* prextern int() function in Buzz
|
||||||
|
* This function is used to print data from buzz
|
||||||
|
* The command to use in Buzz is buzzros_print takes any available datatype in Buzz
|
||||||
|
*/
|
||||||
|
int buzzros_print(buzzvm_t vm);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* buzzuav_goto(latitude,longitude,altitude) function in Buzz
|
||||||
|
* commands the UAV to go to a position supplied
|
||||||
|
*/
|
||||||
|
int buzzuav_goto(buzzvm_t vm);
|
||||||
|
/* Returns the current command from local variable*/
|
||||||
|
int getcmd();
|
||||||
|
/*Sets goto position could be used for bypassing*/
|
||||||
|
void set_goto(double pos[]);
|
||||||
|
/*sets rc requested command */
|
||||||
|
void rc_call(int rc_cmd);
|
||||||
|
/* sets the battery state */
|
||||||
|
void set_battery(float voltage,float current,float remaining);
|
||||||
|
/*retuns the current go to position */
|
||||||
|
double* getgoto();
|
||||||
|
/*
|
||||||
|
* Commands the UAV to takeoff
|
||||||
|
*/
|
||||||
|
int buzzuav_takeoff(buzzvm_t vm);
|
||||||
|
|
||||||
|
/* Commands the UAV to land
|
||||||
|
*/
|
||||||
|
int buzzuav_land(buzzvm_t vm);
|
||||||
|
|
||||||
|
/* Command the UAV to go to home location
|
||||||
|
*/
|
||||||
|
int buzzuav_gohome(buzzvm_t vm);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Updates battery information in Buzz
|
||||||
|
*/
|
||||||
|
int buzzuav_update_battery(buzzvm_t vm);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Updates IR information in Buzz
|
||||||
|
* Proximity and ground sensors to do !!!!
|
||||||
|
*/
|
||||||
|
int buzzuav_update_prox(buzzvm_t vm);
|
||||||
|
|
||||||
|
//#endif
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,103 @@
|
||||||
|
#pragma once
|
||||||
|
#include "ros/ros.h"
|
||||||
|
#include "sensor_msgs/NavSatFix.h"
|
||||||
|
#include "mavros_msgs/GlobalPositionTarget.h"
|
||||||
|
#include "mavros_msgs/CommandCode.h"
|
||||||
|
#include "mavros_msgs/CommandInt.h"
|
||||||
|
#include "mavros_msgs/State.h"
|
||||||
|
#include "mavros_msgs/BatteryStatus.h"
|
||||||
|
#include "mavros_msgs/Mavlink.h"
|
||||||
|
#include "sensor_msgs/NavSatStatus.h"
|
||||||
|
#include <sstream>
|
||||||
|
#include <buzz/buzzasm.h>
|
||||||
|
#include "buzzuav_closures.h"
|
||||||
|
#include "buzz_utility.h"
|
||||||
|
#include "uav_utility.h"
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <math.h>
|
||||||
|
#include <signal.h>
|
||||||
|
#include <ostream>
|
||||||
|
#include <map>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
|
||||||
|
namespace rosbzz_node{
|
||||||
|
|
||||||
|
|
||||||
|
class roscontroller{
|
||||||
|
|
||||||
|
public:
|
||||||
|
roscontroller();
|
||||||
|
~roscontroller();
|
||||||
|
//void RosControllerInit();
|
||||||
|
void RosControllerRun();
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
double cur_pos[3];
|
||||||
|
uint64_t payload;
|
||||||
|
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
|
||||||
|
int timer_step=0;
|
||||||
|
int robot_id=0;
|
||||||
|
int oldcmdID=0;
|
||||||
|
int rc_cmd;
|
||||||
|
std::string bzzfile_name, fcclient_name, rcservice_name; //, rcclient;
|
||||||
|
bool rcclient;
|
||||||
|
ros::ServiceClient mav_client;
|
||||||
|
ros::Publisher payload_pub;
|
||||||
|
ros::ServiceServer service;
|
||||||
|
ros::Subscriber current_position_sub;
|
||||||
|
ros::Subscriber battery_sub;
|
||||||
|
ros::Subscriber payload_sub;
|
||||||
|
/*Create node Handler*/
|
||||||
|
ros::NodeHandle n_c;
|
||||||
|
/*Commands for flight controller*/
|
||||||
|
mavros_msgs::CommandInt cmd_srv;
|
||||||
|
/* The bytecode filename */
|
||||||
|
char* bcfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
|
||||||
|
/* The debugging information file name */
|
||||||
|
char* dbgfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
|
||||||
|
|
||||||
|
void Initialize_pub_sub();
|
||||||
|
|
||||||
|
/*Obtain data from ros parameter server*/
|
||||||
|
void Rosparameters_get();
|
||||||
|
|
||||||
|
void Compile_bzz();
|
||||||
|
|
||||||
|
/*Prepare messages and publish*/
|
||||||
|
inline void prepare_msg_and_publish();
|
||||||
|
|
||||||
|
|
||||||
|
/*Refresh neighbours Position for every ten step*/
|
||||||
|
inline void maintain_pos(int tim_step);
|
||||||
|
|
||||||
|
/*Maintain neighbours position*/
|
||||||
|
inline void neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr );
|
||||||
|
|
||||||
|
/*Set the current position of the robot callback*/
|
||||||
|
inline void set_cur_pos(double latitude,
|
||||||
|
double longitude,
|
||||||
|
double altitude);
|
||||||
|
/*convert from catresian to spherical coordinate system callback */
|
||||||
|
inline double* cvt_spherical_coordinates(double neighbours_pos_payload []);
|
||||||
|
|
||||||
|
/*battery status callback*/
|
||||||
|
inline void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
|
||||||
|
|
||||||
|
/*current position callback*/
|
||||||
|
inline void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
|
||||||
|
|
||||||
|
/*payload callback callback*/
|
||||||
|
inline void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
|
||||||
|
|
||||||
|
/* RC commands service */
|
||||||
|
inline bool rc_callback(mavros_msgs::CommandInt::Request &req,
|
||||||
|
mavros_msgs::CommandInt::Response &res);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
|
|
@ -40,6 +40,14 @@
|
||||||
<!-- Use test_depend for packages you need only for testing: -->
|
<!-- Use test_depend for packages you need only for testing: -->
|
||||||
<!-- <test_depend>gtest</test_depend> -->
|
<!-- <test_depend>gtest</test_depend> -->
|
||||||
<buildtool_depend>catkin</buildtool_depend>
|
<buildtool_depend>catkin</buildtool_depend>
|
||||||
|
<build_depend>roscpp</build_depend>
|
||||||
|
<run_depend>roscpp</run_depend>
|
||||||
|
<build_depend>std_msgs</build_depend>
|
||||||
|
<run_depend>std_msgs</run_depend>
|
||||||
|
<build_depend>mavros_msgs</build_depend>
|
||||||
|
<run_depend>mavros_msgs</run_depend>
|
||||||
|
<build_depend>sensor_msgs</build_depend>
|
||||||
|
<run_depend>sensor_msgs</run_depend>
|
||||||
<!-- <build_depend>message_generation</build_depend> -->
|
<!-- <build_depend>message_generation</build_depend> -->
|
||||||
<!-- <run_depend>message_runtime</run_depend> -->
|
<!-- <run_depend>message_runtime</run_depend> -->
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -5,17 +5,11 @@
|
||||||
* @author Vivek Shankar Varadharajan
|
* @author Vivek Shankar Varadharajan
|
||||||
* @copyright 2016 MistLab. All rights reserved.
|
* @copyright 2016 MistLab. All rights reserved.
|
||||||
*/
|
*/
|
||||||
#define _GNU_SOURCE
|
|
||||||
#include <stdio.h>
|
|
||||||
#include "buzz_utility.h"
|
#include "buzz_utility.h"
|
||||||
#include "buzzuav_closures.h"
|
|
||||||
#include <buzz/buzzdebug.h>
|
namespace buzz_utility{
|
||||||
#include <stdlib.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <unistd.h>
|
|
||||||
#include <iostream>
|
|
||||||
#include <map>
|
|
||||||
using namespace std;
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
||||||
|
|
@ -23,10 +17,7 @@ static buzzvm_t VM = 0;
|
||||||
static char* BO_FNAME = 0;
|
static char* BO_FNAME = 0;
|
||||||
static uint8_t* BO_BUF = 0;
|
static uint8_t* BO_BUF = 0;
|
||||||
static buzzdebug_t DBG_INFO = 0;
|
static buzzdebug_t DBG_INFO = 0;
|
||||||
static int MSG_SIZE = 32;
|
static unsigned int MSG_SIZE = 32;
|
||||||
static int TCP_LIST_STREAM = -1;
|
|
||||||
static int TCP_COMM_STREAM = -1;
|
|
||||||
static uint8_t* STREAM_SEND_BUF = NULL;
|
|
||||||
|
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
@ -177,19 +168,19 @@ static const char* buzz_error_info() {
|
||||||
|
|
||||||
static int buzz_register_hooks() {
|
static int buzz_register_hooks() {
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
|
||||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzros_print));
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
|
||||||
buzzvm_gstore(VM);
|
buzzvm_gstore(VM);
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
|
||||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_goto));
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto));
|
||||||
buzzvm_gstore(VM);
|
buzzvm_gstore(VM);
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
|
||||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_takeoff));
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff));
|
||||||
buzzvm_gstore(VM);
|
buzzvm_gstore(VM);
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1));
|
||||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_gohome));
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_gohome));
|
||||||
buzzvm_gstore(VM);
|
buzzvm_gstore(VM);
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
|
||||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_land));
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_land));
|
||||||
buzzvm_gstore(VM);
|
buzzvm_gstore(VM);
|
||||||
return BUZZVM_STATE_READY;
|
return BUZZVM_STATE_READY;
|
||||||
}
|
}
|
||||||
|
|
@ -307,8 +298,8 @@ void buzz_script_step() {
|
||||||
/*
|
/*
|
||||||
* Update sensors
|
* Update sensors
|
||||||
*/
|
*/
|
||||||
buzzuav_update_battery(VM);
|
buzzuav_closures::buzzuav_update_battery(VM);
|
||||||
buzzuav_update_prox(VM);
|
buzzuav_closures::buzzuav_update_prox(VM);
|
||||||
/*
|
/*
|
||||||
* Call Buzz step() function
|
* Call Buzz step() function
|
||||||
*/
|
*/
|
||||||
|
|
@ -351,6 +342,10 @@ void buzz_script_destroy() {
|
||||||
fprintf(stdout, "Script execution stopped.\n");
|
fprintf(stdout, "Script execution stopped.\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/****************************************/
|
||||||
|
/****************************************/
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
/*Execution completed*/
|
/*Execution completed*/
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
@ -359,7 +354,7 @@ int buzz_script_done() {
|
||||||
return VM->state != BUZZVM_STATE_READY;
|
return VM->state != BUZZVM_STATE_READY;
|
||||||
}
|
}
|
||||||
|
|
||||||
/****************************************/
|
}
|
||||||
/****************************************/
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,13 +1,25 @@
|
||||||
#ifndef BUZZ_UTILITY_H
|
#pragma once
|
||||||
#define BUZZ_UTILITY_H
|
#include <stdio.h>
|
||||||
|
#include "buzz_utility.h"
|
||||||
|
#include "buzzuav_closures.h"
|
||||||
|
#include <buzz/buzzdebug.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <iostream>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <map>
|
#include <map>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
namespace buzz_utility{
|
||||||
|
|
||||||
struct pos_struct
|
struct pos_struct
|
||||||
{
|
{
|
||||||
double x,y,z;
|
double x,y,z;
|
||||||
pos_struct(double x,double y,double z):x(x),y(y),z(z){};
|
pos_struct(double x,double y,double z):x(x),y(y),z(z){};
|
||||||
pos_struct(){}
|
pos_struct(){}
|
||||||
};
|
};
|
||||||
|
|
||||||
typedef struct pos_struct Pos_struct ;
|
typedef struct pos_struct Pos_struct ;
|
||||||
|
|
||||||
uint16_t* u64_cvt_u16(uint64_t u64);
|
uint16_t* u64_cvt_u16(uint64_t u64);
|
||||||
|
|
@ -29,4 +41,4 @@ void buzz_script_destroy();
|
||||||
|
|
||||||
int buzz_script_done();
|
int buzz_script_done();
|
||||||
|
|
||||||
#endif
|
}
|
||||||
|
|
|
||||||
|
|
@ -1,56 +0,0 @@
|
||||||
#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
|
|
||||||
|
|
@ -6,15 +6,12 @@
|
||||||
* @copyright 2016 MistLab. All rights reserved.
|
* @copyright 2016 MistLab. All rights reserved.
|
||||||
*/
|
*/
|
||||||
//#define _GNU_SOURCE
|
//#define _GNU_SOURCE
|
||||||
#include <stdio.h>
|
|
||||||
#include "buzzuav_closures.h"
|
#include "buzzuav_closures.h"
|
||||||
#include "uav_utility.h"
|
namespace buzzuav_closures{
|
||||||
#include "mavros_msgs/CommandCode.h"
|
static double goto_pos[3];
|
||||||
#include "ros/ros.h"
|
static float batt[3];
|
||||||
|
static int cur_cmd;
|
||||||
double goto_pos[3];
|
static int rc_cmd=0;
|
||||||
float batt[3];
|
|
||||||
int cur_cmd;
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
||||||
|
|
@ -84,8 +81,13 @@ return goto_pos;
|
||||||
}
|
}
|
||||||
/******************************/
|
/******************************/
|
||||||
int getcmd(){
|
int getcmd(){
|
||||||
|
if(rc_cmd==0) return cur_cmd;
|
||||||
|
else {
|
||||||
|
cur_cmd=rc_cmd;
|
||||||
|
rc_cmd=0;
|
||||||
return cur_cmd;
|
return cur_cmd;
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void set_goto(double pos[]){
|
void set_goto(double pos[]){
|
||||||
goto_pos[0]=pos[0];
|
goto_pos[0]=pos[0];
|
||||||
|
|
@ -94,8 +96,8 @@ goto_pos[2]=pos[2];
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void rc_call(int rc_cmd){
|
void rc_call(int rc_cmd_in){
|
||||||
cur_cmd=rc_cmd;
|
rc_cmd=rc_cmd_in;
|
||||||
}
|
}
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
@ -128,7 +130,7 @@ void set_battery(float voltage,float current,float remaining){
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
||||||
int buzzuav_update_battery(buzzvm_t vm) {
|
int buzzuav_update_battery(buzzvm_t vm) {
|
||||||
static char BATTERY_BUF[256];
|
//static char BATTERY_BUF[256];
|
||||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1));
|
buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1));
|
||||||
buzzvm_pusht(vm);
|
buzzvm_pusht(vm);
|
||||||
buzzvm_dup(vm);
|
buzzvm_dup(vm);
|
||||||
|
|
@ -172,9 +174,11 @@ int buzzuav_update_prox(buzzvm_t vm) {
|
||||||
buzzvm_pushi(vm, (PROXIMITY_BUF[i*2] | PROXIMITY_BUF[i*2+1] << 8));
|
buzzvm_pushi(vm, (PROXIMITY_BUF[i*2] | PROXIMITY_BUF[i*2+1] << 8));
|
||||||
buzzvm_tput(vm);
|
buzzvm_tput(vm);
|
||||||
}
|
}
|
||||||
buzzvm_gstore(vm);
|
buzzvm_gstore(vm);*/
|
||||||
return vm->state;*/
|
return vm->state;
|
||||||
}
|
}
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
||||||
|
}
|
||||||
|
|
|
||||||
|
|
@ -1,7 +1,13 @@
|
||||||
#ifndef BUZZUAV_CLOSURES_H
|
#pragma once
|
||||||
#define BUZZUAV_CLOSURES_H
|
//#ifndef BUZZUAV_CLOSURES_H
|
||||||
|
//#define BUZZUAV_CLOSURES_H
|
||||||
#include <buzz/buzzvm.h>
|
#include <buzz/buzzvm.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include "uav_utility.h"
|
||||||
|
#include "mavros_msgs/CommandCode.h"
|
||||||
|
#include "ros/ros.h"
|
||||||
|
|
||||||
|
namespace buzzuav_closures{
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* prextern int() function in Buzz
|
* prextern int() function in Buzz
|
||||||
|
|
@ -49,4 +55,5 @@ int buzzuav_update_battery(buzzvm_t vm);
|
||||||
*/
|
*/
|
||||||
int buzzuav_update_prox(buzzvm_t vm);
|
int buzzuav_update_prox(buzzvm_t vm);
|
||||||
|
|
||||||
#endif
|
//#endif
|
||||||
|
}
|
||||||
|
|
|
||||||
314
src/rosbuzz.cpp
314
src/rosbuzz.cpp
|
|
@ -6,325 +6,19 @@
|
||||||
* @copyright 2016 MistLab. All rights reserved.
|
* @copyright 2016 MistLab. All rights reserved.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "ros/ros.h"
|
#include "roscontroller.h"
|
||||||
#include "sensor_msgs/NavSatFix.h"
|
|
||||||
#include "mavros_msgs/GlobalPositionTarget.h"
|
|
||||||
#include "mavros_msgs/CommandCode.h"
|
|
||||||
#include "mavros_msgs/CommandInt.h"
|
|
||||||
#include "mavros_msgs/State.h"
|
|
||||||
#include "mavros_msgs/BatteryStatus.h"
|
|
||||||
#include "mavros_msgs/Mavlink.h"
|
|
||||||
#include "sensor_msgs/NavSatStatus.h"
|
|
||||||
#include <sstream>
|
|
||||||
#include <buzz/buzzasm.h>
|
|
||||||
#include "buzzuav_closures.h"
|
|
||||||
#include "buzz_utility.h"
|
|
||||||
#include "uav_utility.h"
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <math.h>
|
|
||||||
#include <signal.h>
|
|
||||||
#include <ostream>
|
|
||||||
#include <map>
|
|
||||||
using namespace std;
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* This program implements Buzz node in ros using mavros_msgs.
|
* This program implements Buzz node in ros using mavros_msgs.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
static int done = 0;
|
|
||||||
static double cur_pos[3];
|
|
||||||
static uint64_t payload;
|
|
||||||
static std::map< int, Pos_struct> neighbours_pos_map;
|
|
||||||
static int timer_step=0;
|
|
||||||
static int robot_id=0;
|
|
||||||
|
|
||||||
/*Refresh neighbours Position for every ten step*/
|
|
||||||
void maintain_pos(int tim_step){
|
|
||||||
if(timer_step >=10){
|
|
||||||
neighbours_pos_map.clear();
|
|
||||||
timer_step=0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/*Maintain neighbours position*/
|
|
||||||
void neighbours_pos_maintain(int id, Pos_struct pos_arr ){
|
|
||||||
map< int, Pos_struct >::iterator it = neighbours_pos_map.find(id);
|
|
||||||
if(it!=neighbours_pos_map.end())
|
|
||||||
neighbours_pos_map.erase(it);
|
|
||||||
neighbours_pos_map.insert(make_pair(id, pos_arr));
|
|
||||||
}
|
|
||||||
|
|
||||||
/*print usage information not needed at the moment*/
|
|
||||||
void usage(const char* path, int status) {
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
/*Set the current position of the robot callback*/
|
|
||||||
void set_cur_pos(double latitude,
|
|
||||||
double longitude,
|
|
||||||
double altitude){
|
|
||||||
cur_pos [0] =latitude;
|
|
||||||
cur_pos [1] =longitude;
|
|
||||||
cur_pos [2] =altitude;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
/*convert from catresian to spherical coordinate system callback */
|
|
||||||
double* cvt_spherical_coordinates(double neighbours_pos_payload []){
|
|
||||||
|
|
||||||
double latitude,longitude,altitude;
|
|
||||||
latitude=neighbours_pos_payload[0];
|
|
||||||
longitude = neighbours_pos_payload[1];
|
|
||||||
altitude=neighbours_pos_payload[2];
|
|
||||||
neighbours_pos_payload[0]=sqrt(pow(latitude,2.0)+pow(longitude,2.0)+pow(altitude,2.0));
|
|
||||||
neighbours_pos_payload[1]=atan(longitude/latitude);
|
|
||||||
neighbours_pos_payload[2]=atan((sqrt(pow(latitude,2.0)+pow(longitude,2.0)))/altitude);
|
|
||||||
return neighbours_pos_payload;
|
|
||||||
}
|
|
||||||
|
|
||||||
/*battery status callback*/
|
|
||||||
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg)
|
|
||||||
{
|
|
||||||
set_battery(msg->voltage,msg->current,msg->remaining);
|
|
||||||
}
|
|
||||||
|
|
||||||
/*current position callback*/
|
|
||||||
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg)
|
|
||||||
{
|
|
||||||
set_cur_pos(msg->latitude,msg->longitude,msg->altitude);
|
|
||||||
}
|
|
||||||
|
|
||||||
/*payload callback callback*/
|
|
||||||
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
|
|
||||||
{
|
|
||||||
uint64_t message_obt[msg->payload64.size()];
|
|
||||||
int i = 0;
|
|
||||||
|
|
||||||
/* Go throught the obtained payload*/
|
|
||||||
for(std::vector<long unsigned int>::const_iterator it = msg->payload64.begin(); it != msg->payload64.end(); ++it)
|
|
||||||
{
|
|
||||||
message_obt[i] =(uint64_t) *it;
|
|
||||||
//cout<<"[Debug:] obtaind message "<<message_obt[i]<<endl;
|
|
||||||
i++;
|
|
||||||
}
|
|
||||||
/* Extract neighbours position from payload*/
|
|
||||||
double neighbours_pos_payload[3];
|
|
||||||
memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t));
|
|
||||||
/*Convert obtained position to relative position*/
|
|
||||||
for(i=0;i<3;i++){
|
|
||||||
neighbours_pos_payload[i]=neighbours_pos_payload[i]-cur_pos[i];
|
|
||||||
}
|
|
||||||
double* cvt_neighbours_pos_payload = cvt_spherical_coordinates(neighbours_pos_payload);
|
|
||||||
/*Extract robot id of the neighbour*/
|
|
||||||
uint16_t* out = u64_cvt_u16((uint64_t)*(message_obt+3));
|
|
||||||
/*pass neighbour position to local maintaner*/
|
|
||||||
Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]);
|
|
||||||
neighbours_pos_maintain((int)out[1],n_pos);
|
|
||||||
delete[] out;
|
|
||||||
in_msg_process((message_obt+3));
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
/*Dji RC commands service */
|
|
||||||
int oldcmdID=0;
|
|
||||||
int rc_cmd;
|
|
||||||
bool rc_callback(mavros_msgs::CommandInt::Request &req,
|
|
||||||
mavros_msgs::CommandInt::Response &res){
|
|
||||||
|
|
||||||
if(req.command==oldcmdID)
|
|
||||||
return true;
|
|
||||||
else
|
|
||||||
oldcmdID=req.command;
|
|
||||||
switch(req.command)
|
|
||||||
{
|
|
||||||
case mavros_msgs::CommandCode::NAV_TAKEOFF:
|
|
||||||
ROS_INFO("RC_call: TAKE OFF!!!!");
|
|
||||||
rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
|
|
||||||
rc_call(rc_cmd);
|
|
||||||
res.success = true;
|
|
||||||
break;
|
|
||||||
case mavros_msgs::CommandCode::NAV_LAND:
|
|
||||||
ROS_INFO("RC_Call: LAND!!!!");
|
|
||||||
rc_cmd=mavros_msgs::CommandCode::NAV_LAND;
|
|
||||||
rc_call(rc_cmd);
|
|
||||||
res.success = true;
|
|
||||||
break;
|
|
||||||
case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
|
|
||||||
ROS_INFO("RC_Call: GO HOME!!!!");
|
|
||||||
rc_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
|
|
||||||
rc_call(rc_cmd);
|
|
||||||
res.success = true;
|
|
||||||
break;
|
|
||||||
case mavros_msgs::CommandCode::NAV_WAYPOINT:
|
|
||||||
ROS_INFO("RC_Call: GO TO!!!! x = %f , y = %f , Z = %f",req.param1,req.param2,req.param3);
|
|
||||||
double rc_goto[3];
|
|
||||||
rc_goto[0]=req.param1;
|
|
||||||
rc_goto[1]=req.param2;
|
|
||||||
rc_goto[2]=req.param3;
|
|
||||||
|
|
||||||
set_goto(rc_goto);
|
|
||||||
rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
|
|
||||||
rc_call(rc_cmd);
|
|
||||||
res.success = true;
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
res.success = false;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/*control c handler callback*/
|
|
||||||
static void ctrlc_handler(int sig) {
|
|
||||||
done = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
int main(int argc, char **argv)
|
int main(int argc, char **argv)
|
||||||
{
|
{
|
||||||
|
/*Initialize rosBuzz node*/
|
||||||
ros::ServiceServer service;
|
|
||||||
/*initiate rosBuzz*/
|
|
||||||
ros::init(argc, argv, "rosBuzz");
|
ros::init(argc, argv, "rosBuzz");
|
||||||
ROS_INFO("Buzz_node");
|
rosbzz_node::roscontroller RosController;
|
||||||
|
RosController.RosControllerRun();
|
||||||
/*Create node Handler*/
|
|
||||||
ros::NodeHandle n_c;
|
|
||||||
std::string bzzfile_name, fcclient_name, rcservice_name; //, rcclient;
|
|
||||||
bool rcclient;
|
|
||||||
/*Obtain .bzz file name from parameter server*/
|
|
||||||
if(ros::param::get("/rosbuzz_node/bzzfile_name", bzzfile_name));
|
|
||||||
else {ROS_ERROR("Provide a .bzz file to run in Launch file"); system("rosnode kill rosbuzz_node");}
|
|
||||||
|
|
||||||
/*Obtain rc service option from parameter server*/
|
|
||||||
if(ros::param::get("/rosbuzz_node/rcclient", rcclient)){
|
|
||||||
if(rcclient==true){
|
|
||||||
/*Service*/
|
|
||||||
if(ros::param::get("/rosbuzz_node/rcservice_name", rcservice_name)){
|
|
||||||
service = n_c.advertiseService(rcservice_name, rc_callback);
|
|
||||||
ROS_INFO("Ready to receive Mav Commands from RC client");
|
|
||||||
}
|
|
||||||
else{ROS_ERROR("Provide a name topic name for rc service in Launch file"); system("rosnode kill rosbuzz_node");}
|
|
||||||
}
|
|
||||||
else if(rcclient==false){ROS_INFO("RC service is disabled");}
|
|
||||||
}
|
|
||||||
else{ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node");}
|
|
||||||
/*Obtain fc client name from parameter server*/
|
|
||||||
if(ros::param::get("/rosbuzz_node/fcclient_name", fcclient_name));
|
|
||||||
else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");}
|
|
||||||
/*Obtain robot_id from parameter server*/
|
|
||||||
ros::param::get("/rosbuzz_node/robot_id", robot_id);
|
|
||||||
|
|
||||||
/*subscribers*/
|
|
||||||
ros::Subscriber current_position_sub = n_c.subscribe("current_pos", 1000, current_pos);
|
|
||||||
|
|
||||||
ros::Subscriber battery_sub = n_c.subscribe("battery_state", 1000, battery);
|
|
||||||
|
|
||||||
ros::Subscriber payload_sub = n_c.subscribe("inMavlink", 1000, payload_obt);
|
|
||||||
|
|
||||||
|
|
||||||
/*publishers*/
|
|
||||||
ros::Publisher payload_pub = n_c.advertise<mavros_msgs::Mavlink>("outMavlink", 1000);
|
|
||||||
|
|
||||||
/* Clients*/
|
|
||||||
ros::ServiceClient mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
|
|
||||||
cout<< " rc client name"<<rcservice_name;
|
|
||||||
|
|
||||||
|
|
||||||
/*loop rate of ros*/
|
|
||||||
ros::Rate loop_rate(1);
|
|
||||||
|
|
||||||
/*Compile the buzz code .bzz to .bo*/
|
|
||||||
stringstream bzzfile_in_compile;
|
|
||||||
bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" ../catkin_ws/src/rosbuzz/src/out.basm";
|
|
||||||
//system("rm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
|
|
||||||
system(bzzfile_in_compile.str().c_str());
|
|
||||||
system("bzzasm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
|
|
||||||
|
|
||||||
/* The bytecode filename */
|
|
||||||
char* bcfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
|
|
||||||
/* The debugging information file name */
|
|
||||||
char* dbgfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
|
|
||||||
/* Set CTRL-C handler */
|
|
||||||
signal(SIGTERM, ctrlc_handler);
|
|
||||||
signal(SIGINT, ctrlc_handler);
|
|
||||||
|
|
||||||
|
|
||||||
/* Set the Buzz bytecode */
|
|
||||||
if(buzz_script_set(bcfname, dbgfname,robot_id)) {
|
|
||||||
fprintf(stdout, "Bytecode file found and set\n");
|
|
||||||
|
|
||||||
/*Commands for dji flight controller*/
|
|
||||||
mavros_msgs::CommandInt cmd_srv;
|
|
||||||
|
|
||||||
int count = 0;
|
|
||||||
while (ros::ok() && !done && !buzz_script_done())
|
|
||||||
{
|
|
||||||
|
|
||||||
/*Update neighbors position inside Buzz*/
|
|
||||||
neighbour_pos_callback(neighbours_pos_map);
|
|
||||||
|
|
||||||
/* Main loop */
|
|
||||||
buzz_script_step();
|
|
||||||
|
|
||||||
/*prepare the goto publish message */
|
|
||||||
|
|
||||||
double* goto_pos = getgoto();
|
|
||||||
cmd_srv.request.param1 = goto_pos[0];
|
|
||||||
cmd_srv.request.param2 = goto_pos[1];
|
|
||||||
cmd_srv.request.param3 = goto_pos[2];
|
|
||||||
cmd_srv.request.command = getcmd();
|
|
||||||
/* diji client call*/
|
|
||||||
if (mav_client.call(cmd_srv)){ ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
|
|
||||||
else{ ROS_ERROR("Failed to call service 'djicmd'"); }
|
|
||||||
/*obtain Pay load to be sent*/
|
|
||||||
uint64_t* payload_out_ptr= out_msg_process();
|
|
||||||
uint64_t position[3];
|
|
||||||
/*Appened current position to message*/
|
|
||||||
memcpy(position, cur_pos, 3*sizeof(uint64_t));
|
|
||||||
mavros_msgs::Mavlink payload_out;
|
|
||||||
payload_out.payload64.push_back(position[0]);
|
|
||||||
payload_out.payload64.push_back(position[1]);
|
|
||||||
payload_out.payload64.push_back(position[2]);
|
|
||||||
/*Append Buzz message*/
|
|
||||||
uint16_t* out = u64_cvt_u16(payload_out_ptr[0]);
|
|
||||||
for(int i=0;i<out[0];i++){
|
|
||||||
payload_out.payload64.push_back(payload_out_ptr[i]);
|
|
||||||
}
|
|
||||||
int i=0;
|
|
||||||
for(std::vector<long unsigned int>::const_iterator it = payload_out.payload64.begin(); it != payload_out.payload64.end(); ++it)
|
|
||||||
{
|
|
||||||
//message_obt[i] =(uint64_t) *it;
|
|
||||||
//cout<<" [Debug:] sent message "<<*it<<endl;
|
|
||||||
i++;
|
|
||||||
}
|
|
||||||
/*publish prepared messages in respective topic*/
|
|
||||||
payload_pub.publish(payload_out);
|
|
||||||
delete[] out;
|
|
||||||
delete[] payload_out_ptr;
|
|
||||||
/*run once*/
|
|
||||||
ros::spinOnce();
|
|
||||||
/*sleep for the mentioned loop rate*/
|
|
||||||
loop_rate.sleep();
|
|
||||||
++count;
|
|
||||||
timer_step+=1;
|
|
||||||
maintain_pos(timer_step);
|
|
||||||
|
|
||||||
}
|
|
||||||
/* Cleanup */
|
|
||||||
buzz_script_destroy();
|
|
||||||
|
|
||||||
/* Stop the robot */
|
|
||||||
uav_done();
|
|
||||||
|
|
||||||
}
|
|
||||||
/* All done */
|
|
||||||
return 0;
|
return 0;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,264 @@
|
||||||
|
|
||||||
|
#include "roscontroller.h"
|
||||||
|
|
||||||
|
|
||||||
|
namespace rosbzz_node{
|
||||||
|
|
||||||
|
/***Constructor***/
|
||||||
|
roscontroller::roscontroller()
|
||||||
|
{
|
||||||
|
ROS_INFO("Buzz_node");
|
||||||
|
/*Obtain parameters from ros parameter server*/
|
||||||
|
Rosparameters_get();
|
||||||
|
/*Initialize publishers, subscribers and client*/
|
||||||
|
Initialize_pub_sub();
|
||||||
|
/*Compile the .bzz file to .basm, .bo and .bdbg*/
|
||||||
|
Compile_bzz();
|
||||||
|
}
|
||||||
|
|
||||||
|
/***Destructor***/
|
||||||
|
roscontroller::~roscontroller()
|
||||||
|
{
|
||||||
|
/* All done */
|
||||||
|
/* Cleanup */
|
||||||
|
buzz_utility::buzz_script_destroy();
|
||||||
|
/* Stop the robot */
|
||||||
|
uav_done();
|
||||||
|
}
|
||||||
|
|
||||||
|
/*rosbuzz_node run*/
|
||||||
|
void roscontroller::RosControllerRun(){
|
||||||
|
/* Set the Buzz bytecode */
|
||||||
|
if(buzz_utility::buzz_script_set(bcfname, dbgfname,robot_id)) {
|
||||||
|
fprintf(stdout, "Bytecode file found and set\n");
|
||||||
|
while (ros::ok() && !buzz_utility::buzz_script_done())
|
||||||
|
{
|
||||||
|
/*Update neighbors position inside Buzz*/
|
||||||
|
buzz_utility::neighbour_pos_callback(neighbours_pos_map);
|
||||||
|
/*Step buzz script */
|
||||||
|
buzz_utility::buzz_script_step();
|
||||||
|
/*Prepare messages and publish them in respective topic*/
|
||||||
|
prepare_msg_and_publish();
|
||||||
|
/*run once*/
|
||||||
|
ros::spinOnce();
|
||||||
|
/*loop rate of ros*/
|
||||||
|
ros::Rate loop_rate(1);
|
||||||
|
/*sleep for the mentioned loop rate*/
|
||||||
|
loop_rate.sleep();
|
||||||
|
timer_step+=1;
|
||||||
|
maintain_pos(timer_step);
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void roscontroller::Rosparameters_get(){
|
||||||
|
/*Obtain .bzz file name from parameter server*/
|
||||||
|
if(ros::param::get("/rosbuzz_node/bzzfile_name", bzzfile_name));
|
||||||
|
else {ROS_ERROR("Provide a .bzz file to run in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||||
|
/*Obtain rc service option from parameter server*/
|
||||||
|
if(ros::param::get("/rosbuzz_node/rcclient", rcclient)){
|
||||||
|
if(rcclient==true){
|
||||||
|
/*Service*/
|
||||||
|
if(ros::param::get("/rosbuzz_node/rcservice_name", rcservice_name)){
|
||||||
|
service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this);
|
||||||
|
ROS_INFO("Ready to receive Mav Commands from RC client");
|
||||||
|
}
|
||||||
|
else{ROS_ERROR("Provide a name topic name for rc service in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||||
|
}
|
||||||
|
else if(rcclient==false){ROS_INFO("RC service is disabled");}
|
||||||
|
}
|
||||||
|
else{ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||||
|
/*Obtain fc client name from parameter server*/
|
||||||
|
if(ros::param::get("/rosbuzz_node/fcclient_name", fcclient_name));
|
||||||
|
else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||||
|
/*Obtain robot_id from parameter server*/
|
||||||
|
ros::param::get("/rosbuzz_node/robot_id", robot_id);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void roscontroller::Initialize_pub_sub(){
|
||||||
|
/*subscribers*/
|
||||||
|
current_position_sub = n_c.subscribe("current_pos", 1000, &roscontroller::current_pos,this);
|
||||||
|
battery_sub = n_c.subscribe("battery_state", 1000, &roscontroller::battery,this);
|
||||||
|
payload_sub = n_c.subscribe("inMavlink", 1000, &roscontroller::payload_obt,this);
|
||||||
|
/*publishers*/
|
||||||
|
payload_pub = n_c.advertise<mavros_msgs::Mavlink>("outMavlink", 1000);
|
||||||
|
/* Clients*/
|
||||||
|
mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void roscontroller::Compile_bzz(){
|
||||||
|
/*Compile the buzz code .bzz to .bo*/
|
||||||
|
stringstream bzzfile_in_compile;
|
||||||
|
bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" ../catkin_ws/src/rosbuzz/src/out.basm";
|
||||||
|
//system("rm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
|
||||||
|
system(bzzfile_in_compile.str().c_str());
|
||||||
|
system("bzzasm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void roscontroller::prepare_msg_and_publish(){
|
||||||
|
/*prepare the goto publish message */
|
||||||
|
double* goto_pos = buzzuav_closures::getgoto();
|
||||||
|
cmd_srv.request.param1 = goto_pos[0];
|
||||||
|
cmd_srv.request.param2 = goto_pos[1];
|
||||||
|
cmd_srv.request.param3 = goto_pos[2];
|
||||||
|
cmd_srv.request.command = buzzuav_closures::getcmd();
|
||||||
|
/* flight controller client call*/
|
||||||
|
if (mav_client.call(cmd_srv)){ ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
|
||||||
|
else{ ROS_ERROR("Failed to call service 'djicmd'"); }
|
||||||
|
/*obtain Pay load to be sent*/
|
||||||
|
uint64_t* payload_out_ptr= buzz_utility::out_msg_process();
|
||||||
|
uint64_t position[3];
|
||||||
|
/*Appened current position to message*/
|
||||||
|
memcpy(position, cur_pos, 3*sizeof(uint64_t));
|
||||||
|
mavros_msgs::Mavlink payload_out;
|
||||||
|
payload_out.payload64.push_back(position[0]);
|
||||||
|
payload_out.payload64.push_back(position[1]);
|
||||||
|
payload_out.payload64.push_back(position[2]);
|
||||||
|
/*Append Buzz message*/
|
||||||
|
uint16_t* out = buzz_utility::u64_cvt_u16(payload_out_ptr[0]);
|
||||||
|
for(int i=0;i<out[0];i++){
|
||||||
|
payload_out.payload64.push_back(payload_out_ptr[i]);
|
||||||
|
}
|
||||||
|
/*int i=0;
|
||||||
|
uint64_t message_obt[payload_out.payload64.size()];
|
||||||
|
for(std::vector<long unsigned int>::const_iterator it = payload_out.payload64.begin();
|
||||||
|
it != payload_out.payload64.end(); ++it){
|
||||||
|
message_obt[i] =(uint64_t) *it;
|
||||||
|
cout<<" [Debug:] sent message "<<*it<<endl;
|
||||||
|
i++;
|
||||||
|
}*/
|
||||||
|
/*publish prepared messages in respective topic*/
|
||||||
|
payload_pub.publish(payload_out);
|
||||||
|
delete[] out;
|
||||||
|
delete[] payload_out_ptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/*Refresh neighbours Position for every ten step*/
|
||||||
|
inline void roscontroller::maintain_pos(int tim_step){
|
||||||
|
if(timer_step >=10){
|
||||||
|
neighbours_pos_map.clear();
|
||||||
|
timer_step=0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*Maintain neighbours position*/
|
||||||
|
inline void roscontroller::neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr ){
|
||||||
|
map< int, buzz_utility::Pos_struct >::iterator it = neighbours_pos_map.find(id);
|
||||||
|
if(it!=neighbours_pos_map.end())
|
||||||
|
neighbours_pos_map.erase(it);
|
||||||
|
neighbours_pos_map.insert(make_pair(id, pos_arr));
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/*Set the current position of the robot callback*/
|
||||||
|
inline void roscontroller::set_cur_pos(double latitude,
|
||||||
|
double longitude,
|
||||||
|
double altitude){
|
||||||
|
cur_pos [0] =latitude;
|
||||||
|
cur_pos [1] =longitude;
|
||||||
|
cur_pos [2] =altitude;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/*convert from catresian to spherical coordinate system callback */
|
||||||
|
inline double* roscontroller::cvt_spherical_coordinates(double neighbours_pos_payload []){
|
||||||
|
double latitude,longitude,altitude;
|
||||||
|
latitude=neighbours_pos_payload[0];
|
||||||
|
longitude = neighbours_pos_payload[1];
|
||||||
|
altitude=neighbours_pos_payload[2];
|
||||||
|
neighbours_pos_payload[0]=sqrt(pow(latitude,2.0)+pow(longitude,2.0)+pow(altitude,2.0));
|
||||||
|
neighbours_pos_payload[1]=atan(longitude/latitude);
|
||||||
|
neighbours_pos_payload[2]=atan((sqrt(pow(latitude,2.0)+pow(longitude,2.0)))/altitude);
|
||||||
|
return neighbours_pos_payload;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*battery status callback*/
|
||||||
|
inline void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg){
|
||||||
|
buzzuav_closures::set_battery(msg->voltage,msg->current,msg->remaining);
|
||||||
|
}
|
||||||
|
|
||||||
|
/*current position callback*/
|
||||||
|
inline void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
|
||||||
|
set_cur_pos(msg->latitude,msg->longitude,msg->altitude);
|
||||||
|
}
|
||||||
|
|
||||||
|
/*payload callback callback*/
|
||||||
|
inline void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg){
|
||||||
|
uint64_t message_obt[msg->payload64.size()];
|
||||||
|
int i = 0;
|
||||||
|
|
||||||
|
/* Go throught the obtained payload*/
|
||||||
|
for(std::vector<long unsigned int>::const_iterator it = msg->payload64.begin(); it != msg->payload64.end(); ++it){
|
||||||
|
message_obt[i] =(uint64_t) *it;
|
||||||
|
//cout<<"[Debug:] obtaind message "<<message_obt[i]<<endl;
|
||||||
|
i++;
|
||||||
|
}
|
||||||
|
/* Extract neighbours position from payload*/
|
||||||
|
double neighbours_pos_payload[3];
|
||||||
|
memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t));
|
||||||
|
/*Convert obtained position to relative position*/
|
||||||
|
for(i=0;i<3;i++){
|
||||||
|
neighbours_pos_payload[i]=neighbours_pos_payload[i]-cur_pos[i];
|
||||||
|
}
|
||||||
|
double* cvt_neighbours_pos_payload = cvt_spherical_coordinates(neighbours_pos_payload);
|
||||||
|
/*Extract robot id of the neighbour*/
|
||||||
|
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3));
|
||||||
|
/*pass neighbour position to local maintaner*/
|
||||||
|
buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]);
|
||||||
|
neighbours_pos_maintain((int)out[1],n_pos);
|
||||||
|
delete[] out;
|
||||||
|
buzz_utility::in_msg_process((message_obt+3));
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/* RC command service */
|
||||||
|
inline bool roscontroller::rc_callback(mavros_msgs::CommandInt::Request &req,
|
||||||
|
mavros_msgs::CommandInt::Response &res){
|
||||||
|
int rc_cmd;
|
||||||
|
if(req.command==oldcmdID)
|
||||||
|
return true;
|
||||||
|
else oldcmdID=req.command;
|
||||||
|
switch(req.command){
|
||||||
|
case mavros_msgs::CommandCode::NAV_TAKEOFF:
|
||||||
|
ROS_INFO("RC_call: TAKE OFF!!!!");
|
||||||
|
rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
|
||||||
|
buzzuav_closures::rc_call(rc_cmd);
|
||||||
|
res.success = true;
|
||||||
|
break;
|
||||||
|
case mavros_msgs::CommandCode::NAV_LAND:
|
||||||
|
ROS_INFO("RC_Call: LAND!!!!");
|
||||||
|
rc_cmd=mavros_msgs::CommandCode::NAV_LAND;
|
||||||
|
buzzuav_closures::rc_call(rc_cmd);
|
||||||
|
res.success = true;
|
||||||
|
break;
|
||||||
|
case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
|
||||||
|
ROS_INFO("RC_Call: GO HOME!!!!");
|
||||||
|
rc_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
|
||||||
|
buzzuav_closures::rc_call(rc_cmd);
|
||||||
|
res.success = true;
|
||||||
|
break;
|
||||||
|
case mavros_msgs::CommandCode::NAV_WAYPOINT:
|
||||||
|
ROS_INFO("RC_Call: GO TO!!!! x = %f , y = %f , Z = %f",req.param1,req.param2,req.param3);
|
||||||
|
double rc_goto[3];
|
||||||
|
rc_goto[0]=req.param1;
|
||||||
|
rc_goto[1]=req.param2;
|
||||||
|
rc_goto[2]=req.param3;
|
||||||
|
buzzuav_closures::set_goto(rc_goto);
|
||||||
|
rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
|
||||||
|
buzzuav_closures::rc_call(rc_cmd);
|
||||||
|
res.success = true;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
res.success = false;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,101 @@
|
||||||
|
#pragma once
|
||||||
|
#include "ros/ros.h"
|
||||||
|
#include "sensor_msgs/NavSatFix.h"
|
||||||
|
#include "mavros_msgs/GlobalPositionTarget.h"
|
||||||
|
#include "mavros_msgs/CommandCode.h"
|
||||||
|
#include "mavros_msgs/CommandInt.h"
|
||||||
|
#include "mavros_msgs/State.h"
|
||||||
|
#include "mavros_msgs/BatteryStatus.h"
|
||||||
|
#include "mavros_msgs/Mavlink.h"
|
||||||
|
#include "sensor_msgs/NavSatStatus.h"
|
||||||
|
#include <sstream>
|
||||||
|
#include <buzz/buzzasm.h>
|
||||||
|
#include "buzzuav_closures.h"
|
||||||
|
#include "buzz_utility.h"
|
||||||
|
#include "uav_utility.h"
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <math.h>
|
||||||
|
#include <signal.h>
|
||||||
|
#include <ostream>
|
||||||
|
#include <map>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
|
||||||
|
namespace rosbzz_node{
|
||||||
|
|
||||||
|
|
||||||
|
class roscontroller{
|
||||||
|
|
||||||
|
public:
|
||||||
|
roscontroller();
|
||||||
|
~roscontroller();
|
||||||
|
void RosControllerRun();
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
double cur_pos[3];
|
||||||
|
uint64_t payload;
|
||||||
|
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
|
||||||
|
int timer_step=0;
|
||||||
|
int robot_id=0;
|
||||||
|
int oldcmdID=0;
|
||||||
|
std::string bzzfile_name, fcclient_name, rcservice_name; //, rcclient;
|
||||||
|
bool rcclient;
|
||||||
|
ros::ServiceClient mav_client;
|
||||||
|
ros::Publisher payload_pub;
|
||||||
|
ros::ServiceServer service;
|
||||||
|
ros::Subscriber current_position_sub;
|
||||||
|
ros::Subscriber battery_sub;
|
||||||
|
ros::Subscriber payload_sub;
|
||||||
|
/*Create node Handler*/
|
||||||
|
ros::NodeHandle n_c;
|
||||||
|
/*Commands for flight controller*/
|
||||||
|
mavros_msgs::CommandInt cmd_srv;
|
||||||
|
/* The bytecode filename */
|
||||||
|
char* bcfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
|
||||||
|
/* The debugging information file name */
|
||||||
|
char* dbgfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
|
||||||
|
|
||||||
|
void Initialize_pub_sub();
|
||||||
|
|
||||||
|
/*Obtain data from ros parameter server*/
|
||||||
|
void Rosparameters_get();
|
||||||
|
|
||||||
|
void Compile_bzz();
|
||||||
|
|
||||||
|
/*Prepare messages and publish*/
|
||||||
|
inline void prepare_msg_and_publish();
|
||||||
|
|
||||||
|
|
||||||
|
/*Refresh neighbours Position for every ten step*/
|
||||||
|
inline void maintain_pos(int tim_step);
|
||||||
|
|
||||||
|
/*Maintain neighbours position*/
|
||||||
|
inline void neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr );
|
||||||
|
|
||||||
|
/*Set the current position of the robot callback*/
|
||||||
|
inline void set_cur_pos(double latitude,
|
||||||
|
double longitude,
|
||||||
|
double altitude);
|
||||||
|
/*convert from catresian to spherical coordinate system callback */
|
||||||
|
inline double* cvt_spherical_coordinates(double neighbours_pos_payload []);
|
||||||
|
|
||||||
|
/*battery status callback*/
|
||||||
|
inline void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
|
||||||
|
|
||||||
|
/*current position callback*/
|
||||||
|
inline void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
|
||||||
|
|
||||||
|
/*payload callback callback*/
|
||||||
|
inline void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
|
||||||
|
|
||||||
|
/* RC commands service */
|
||||||
|
inline bool rc_callback(mavros_msgs::CommandInt::Request &req,
|
||||||
|
mavros_msgs::CommandInt::Response &res);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
|
|
@ -17,7 +17,7 @@ neighbors.foreach(
|
||||||
"azimuth = ", data.azimuth, ", ",
|
"azimuth = ", data.azimuth, ", ",
|
||||||
"elevation = ", data.elevation) })
|
"elevation = ", data.elevation) })
|
||||||
if(i==1){
|
if(i==1){
|
||||||
s.exec(function() {uav_takeoff()})
|
s.exec(function() {uav_gohome()})
|
||||||
}
|
}
|
||||||
else{
|
else{
|
||||||
s.exec(function() {uav_goto(1.1234,2.2345,3.3456)})
|
s.exec(function() {uav_goto(1.1234,2.2345,3.3456)})
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue