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

force push from ros_drones_ws

This commit is contained in:
dave 2018-09-07 00:17:30 -04:00
parent 0647601086
commit 8cdd5eb105
13 changed files with 200 additions and 230 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
CMakeLists.txt
View File

@ -5,11 +5,8 @@ if(UNIX)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11")
endif()
if(SIM)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=1 -DMAVROSKINETIC=1")
else()
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0 -DMAVROSKINETIC=1")
endif()
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=${SIM} -DMAVROSKINETIC=${KIN}")
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
roscpp
@ -70,6 +67,11 @@ install(TARGETS rosbuzz_node
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Install project namespaced headers
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE)
find_package(catkin REQUIRED COMPONENTS roslaunch)
roslaunch_add_file_check(launch)

7
buzz_scripts/include/act/states.bzz
View File

@ -4,12 +4,14 @@
#
########################################
include "utils/vec2.bzz"
include "act/old_barrier.bzz"
include "act/barrier.bzz"
#include "act/old_barrier.bzz"
include "utils/conversions.bzz"
TARGET_ALTITUDE = 15.0 # m.
BVMSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps
GOTO_MAXVEL = 0.5 # m/steps
GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 1.0 # m.
@ -27,6 +29,7 @@ function idle() {
BVMSTATE = "IDLE"
}
# Custom function for the user.
function cusfun(){
BVMSTATE="CUSFUN"
@ -108,9 +111,7 @@ function goto_gps(transf) {
else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
transf()
else {
log("Waypoint before scale : x ",m_navigation.x, " Y ",m_navigation.y )
m_navigation = LimitSpeed(m_navigation, 1.0)
log("Waypoint after scale : x ",m_navigation.x, " Y ",m_navigation.y )
goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0)
}
}

5
buzz_scripts/include/taskallocate/graphformGPS.bzz
View File

@ -389,7 +389,7 @@ function TransitionToJoined(){
#write statues
#v_tag.put(m_nLabel, m_lockstig)
barrier_create()
barrier_ready(940)
barrier_ready(900+50)
m_navigation.x=0.0
m_navigation.y=0.0
@ -568,7 +568,7 @@ function set_rc_goto() {
goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x)))
print("Saving GPS goal: ",goal.latitude, goal.longitude)
uav_storegoal(goal.latitude, goal.longitude, pose.position.altitude)
storegoal(goal.latitude, goal.longitude, pose.position.altitude)
m_gotjoinedparent = 1
}
}
@ -677,6 +677,7 @@ function DoLock() {
m_navigation=motion_vector()
}
# #move
# goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
BroadcastGraph()

6
buzz_scripts/include/utils/conversions.bzz
View File

@ -44,9 +44,9 @@ function vec_from_gps(lat, lon, home_ref) {
}
ned_x = d_lat/180*math.pi * 6371000.0;
ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi);
m_Lgoal_range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
m_Lgoal_bearing = LimitAngle(math.atan(ned_y,ned_x));
return math.vec2.newp(m_Lgoal_range,m_Lgoal_bearing)
#Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
#Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x));
return math.vec2.new(ned_x,ned_y)
}
function gps_from_vec(vec) {

7
buzz_scripts/main.bzz
View File

@ -27,14 +27,15 @@ goal_list = {
.0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5}
}
# Executed once at init time.
function init() {
init_stig()
init_swarm()
initGraph()
# initGraph()
TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*4.0 # m
loop = 1
# start the swarm command listener
@ -109,10 +110,8 @@ function step() {
# Executed once when the robot (or the simulator) is reset.
function reset() {
resetGraph()
}
# Executed once at the end of experiment.
function destroy() {
destroyGraph()
}

4
include/buzz_utility.h
View File

@ -90,6 +90,8 @@ int update_step_test();
int get_robotid();
int get_swarmsize();
buzzvm_t get_vm();
void set_robot_var(int ROBOTS);
@ -98,7 +100,7 @@ int get_inmsg_size();
std::vector<uint8_t*> get_inmsg_vector();
std::string getuavstate();
std::string get_bvmstate();
int get_timesync_state();

16
include/buzzuav_closures.h
View File

@ -6,6 +6,7 @@
#include "mavros_msgs/Mavlink.h"
#include "ros/ros.h"
#include "buzz_utility.h"
#include "rosbuzz/mavrosCC.h"
#define EARTH_RADIUS (double)6371000.0
#define DEG2RAD(DEG) (double)((DEG) * ((M_PI) / (180.0)))
@ -13,19 +14,6 @@
namespace buzzuav_closures
{
typedef enum {
COMMAND_NIL = 0, // Dummy command
COMMAND_TAKEOFF, // Take off
COMMAND_LAND,
COMMAND_GOHOME,
COMMAND_ARM,
COMMAND_DISARM,
COMMAND_GOTO,
COMMAND_MOVETO,
COMMAND_PICTURE,
COMMAND_GIMBAL,
} Custom_CommandCode;
/*
* prextern int() function in Buzz
* This function is used to print data from buzz
@ -89,6 +77,7 @@ void set_filtered_packet_loss(float value);
/*
* sets current position
*/
void set_currentNEDpos(double x, double y);
void set_currentpos(double latitude, double longitude, float altitude, float yaw);
@ -101,6 +90,7 @@ double* getgoto();
*/
std::map<int, std::map<int,int>> getgrid();
/*
* returns the gimbal commands
*/

24
include/rosbuzz/mavrosCC.h Normal file
View File

@ -0,0 +1,24 @@
#pragma once
#if MAVROSKINETIC
const short MISSION_START = mavros_msgs::CommandCode::MISSION_START;
const short DO_MOUNT_CONTROL = mavros_msgs::CommandCode::DO_MOUNT_CONTROL;
const short COMPONENT_ARM_DISARM = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM;
#else
const short MISSION_START = mavros_msgs::CommandCode::CMD_MISSION_START;
const short DO_MOUNT_CONTROL = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
const short COMPONENT_ARM_DISARM = CMD_COMPONENT_ARM_DISARM;
#endif
const short NAV_TAKEOFF = mavros_msgs::CommandCode::NAV_TAKEOFF;
const short NAV_LAND = mavros_msgs::CommandCode::NAV_LAND;
const short NAV_RETURN_TO_LAUNCH = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
const short NAV_SPLINE_WAYPOINT = mavros_msgs::CommandCode::NAV_SPLINE_WAYPOINT;
const short NAV_WAYPOINT = mavros_msgs::CommandCode::NAV_WAYPOINT;
const short IMAGE_START_CAPTURE = mavros_msgs::CommandCode::IMAGE_START_CAPTURE;
const short CMD_REQUEST_UPDATE = 666;
const short CMD_SYNC_CLOCK = 777;

19
include/roscontroller.h
View File

@ -35,8 +35,8 @@
#include <signal.h>
#include <ostream>
#include <map>
#include <cmath>
#include "buzzuav_closures.h"
#include "rosbuzz/mavrosCC.h"
/*
* ROSBuzz message types
@ -44,7 +44,7 @@
typedef enum {
ROS_BUZZ_MSG_NIL = 0, // dummy msg
UPDATER_MESSAGE, // Update msg
BUZZ_MESSAGE_WTO_TIME, // Broadcast message wihout time info
BUZZ_MESSAGE_NO_TIME, // Broadcast message wihout time info
BUZZ_MESSAGE_TIME, // Broadcast message with time info
} rosbuzz_msgtype;
@ -56,12 +56,10 @@ typedef enum {
#define TIMEOUT 60
#define BUZZRATE 10
#define CMD_REQUEST_UPDATE 666
#define CMD_SYNC_CLOCK 777
using namespace std;
namespace rosbzz_node
namespace rosbuzz_node
{
class roscontroller
{
@ -99,6 +97,8 @@ private:
};
typedef struct POSE ros_pose;
ros_pose target, home, cur_pos;
struct MsgData
{
int msgid;
@ -112,8 +112,6 @@ private:
};
typedef struct MsgData msg_data;
ros_pose target, home, cur_pos;
uint64_t payload;
std::map<int, buzz_utility::Pos_struct> neighbours_pos_map;
std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
@ -133,7 +131,6 @@ private:
int armstate;
int barrier;
int update;
int statepub_active;
int message_number = 0;
uint8_t no_of_robots = 0;
bool rcclient;
@ -141,6 +138,7 @@ private:
bool multi_msg;
uint8_t no_cnt = 0;
uint8_t old_val = 0;
bool debug = false;
std::string bzzfile_name;
std::string fcclient_name;
std::string armclient;
@ -164,7 +162,7 @@ private:
ros::Publisher payload_pub;
ros::Publisher MPpayload_pub;
ros::Publisher neigh_pos_pub;
ros::Publisher uavstate_pub;
ros::Publisher bvmstate_pub;
ros::Publisher grid_pub;
ros::Publisher localsetpoint_nonraw_pub;
ros::ServiceServer service;
@ -212,7 +210,7 @@ private:
void neighbours_pos_publisher();
/*UAVState publisher*/
void uavstate_publisher();
void state_publisher();
/*Grid publisher*/
void grid_publisher();
@ -302,6 +300,7 @@ private:
bool GetRawPacketLoss(const uint8_t short_id, float& result);
bool GetFilteredPacketLoss(const uint8_t short_id, float& result);
void get_xbee_status();
void time_sync_step();
void push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr);
uint64_t get_logical_time();

53
src/buzz_utility.cpp
View File

@ -204,25 +204,25 @@ static int buzz_register_hooks()
buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "storegoal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "setgimbal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_setgimbal));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "takepicture", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "arm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "disarm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_disarm));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "gohome", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_gohome));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
@ -258,25 +258,25 @@ static int testing_buzz_register_hooks()
buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "storegoal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "setgimbal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "takepicture", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "arm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "disarm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "gohome", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
@ -366,7 +366,7 @@ int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_
}
// Initialize UAVSTATE variable
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "BMVSTATE", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
buzzvm_gstore(VM);
@ -426,7 +426,7 @@ int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t
}
// Initialize UAVSTATE variable
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
buzzvm_gstore(VM);
@ -539,14 +539,6 @@ int update_step_test()
return a == BUZZVM_STATE_READY;
}
buzzvm_t get_vm()
/*
/ return the BVM
----------------*/
{
return VM;
}
void set_robot_var(int ROBOTS)
/*
/ set swarm size in the BVM
@ -569,7 +561,15 @@ std::vector<uint8_t*> get_inmsg_vector(){
return IN_MSG;
}
string getuavstate()
buzzvm_t get_vm()
/*
/ return the BVM
----------------*/
{
return VM;
}
string get_bvmstate()
/*
/ return current BVM state
---------------------------------------*/
@ -579,12 +579,19 @@ string getuavstate()
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
buzzvm_gload(VM);
buzzobj_t obj = buzzvm_stack_at(VM, 1);
if(obj->o.type == BUZZTYPE_STRING)
uav_state = string(obj->s.value.str);
else
uav_state = "Not Available";
buzzvm_pop(VM);
}
return uav_state;
}
int get_swarmsize() {
return (int)buzzdict_size(VM->swarmmembers) + 1;
}
int get_timesync_state()
/*
/ return time sync state from bzz script

46
src/buzzuav_closures.cpp
View File

@ -105,10 +105,10 @@ float constrainAngle(float x)
/ Wrap the angle between -pi, pi
----------------------------------------------------------- */
{
x = fmod(x, 2 * M_PI);
x = fmod(x + M_PI, 2 * M_PI);
if (x < 0.0)
x += 2 * M_PI;
return x;
return x - M_PI;
}
void rb_from_gps(double nei[], double out[], double cur[])
@ -229,9 +229,9 @@ int buzzuav_moveto(buzzvm_t vm)
goto_pos[2] = height + dh;
goto_pos[3] = dyaw;
// 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; // TODO: standard mavros?
// 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 = NAV_SPLINE_WAYPOINT; // TODO: standard mavros?
return buzzvm_ret0(vm);
}
@ -332,7 +332,7 @@ int buzzuav_takepicture(buzzvm_t vm)
/ Buzz closure to take a picture here.
/----------------------------------------*/
{
buzz_cmd = COMMAND_PICTURE;
buzz_cmd = IMAGE_START_CAPTURE;
return buzzvm_ret0(vm);
}
@ -356,7 +356,7 @@ int buzzuav_setgimbal(buzzvm_t vm)
rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value;
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;
buzz_cmd = DO_MOUNT_CONTROL;
return buzzvm_ret0(vm);
}
@ -389,10 +389,10 @@ int buzzuav_storegoal(buzzvm_t vm)
double rb[3];
rb_from_gps(goal, rb, cur_pos);
if (fabs(rb[0]) < 150.0)
if (fabs(rb[0]) < 150.0) {
rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]);
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);
}
@ -401,13 +401,9 @@ int buzzuav_arm(buzzvm_t vm)
/ Buzz closure to arm
/---------------------------------------*/
{
#ifdef MAVROSKINETIC
cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM;
#else
cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
#endif
cur_cmd = COMPONENT_ARM_DISARM;
printf(" Buzz requested Arm \n");
buzz_cmd = COMMAND_ARM;
buzz_cmd = cur_cmd;
return buzzvm_ret0(vm);
}
@ -416,13 +412,9 @@ int buzzuav_disarm(buzzvm_t vm)
/ Buzz closure to disarm
/---------------------------------------*/
{
#ifdef MAVROSKINETIC
cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM + 1;
#else
cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
#endif
cur_cmd = COMPONENT_ARM_DISARM + 1;
printf(" Buzz requested Disarm \n");
buzz_cmd = COMMAND_DISARM;
buzz_cmd = cur_cmd;
return buzzvm_ret0(vm);
}
@ -436,9 +428,9 @@ int buzzuav_takeoff(buzzvm_t vm)
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
goto_pos[2] = buzzvm_stack_at(vm, 1)->f.value;
height = goto_pos[2];
cur_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF;
cur_cmd = NAV_TAKEOFF;
printf(" Buzz requested Take off !!! \n");
buzz_cmd = COMMAND_TAKEOFF;
buzz_cmd = cur_cmd;
return buzzvm_ret0(vm);
}
@ -447,9 +439,9 @@ int buzzuav_land(buzzvm_t vm)
/ Buzz closure to land
/-------------------------------------------------------------*/
{
cur_cmd = mavros_msgs::CommandCode::NAV_LAND;
cur_cmd = NAV_LAND;
printf(" Buzz requested Land !!! \n");
buzz_cmd = COMMAND_LAND;
buzz_cmd = cur_cmd;
return buzzvm_ret0(vm);
}
@ -458,9 +450,9 @@ int buzzuav_gohome(buzzvm_t vm)
/ Buzz closure to return Home
/-------------------------------------------------------------*/
{
cur_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
cur_cmd = NAV_RETURN_TO_LAUNCH;
printf(" Buzz requested gohome !!! \n");
buzz_cmd = COMMAND_GOHOME;
buzz_cmd = cur_cmd;
return buzzvm_ret0(vm);
}

2
src/rosbuzz.cpp
View File

@ -17,7 +17,7 @@ int main(int argc, char** argv)
ros::init(argc, argv, "rosBuzz");
ros::NodeHandle nh_priv("~");
ros::NodeHandle nh;
rosbzz_node::roscontroller RosController(nh, nh_priv);
rosbuzz_node::roscontroller RosController(nh, nh_priv);
RosController.RosControllerRun();
return 0;

147
src/roscontroller.cpp
View File

@ -8,10 +8,9 @@
#include "roscontroller.h"
#include <thread>
namespace rosbzz_node
namespace rosbuzz_node
{
const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image";
const bool debug = true;
roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv):
logical_clock(ros::Time()), previous_step_time(ros::Time())
@ -132,23 +131,15 @@ void roscontroller::RosControllerRun()
// set ROS loop rate
ros::Rate loop_rate(BUZZRATE);
// check for BVMSTATE variable
buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
buzzvm_gload(VM);
buzzobj_t obj = buzzvm_stack_at(VM, 1);
if(obj->o.type == BUZZTYPE_STRING) statepub_active = 1;
else
{
statepub_active = 0;
ROS_ERROR("BVMSTATE undeclared in .bzz file, BVMSTATE pusblisher disabled.");
}
if(buzz_utility::get_bvmstate()=="Not Available")
ROS_ERROR("BVMSTATE undeclared in .bzz file, please set BVMSTATE.");
// DEBUG
// ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id);
while (ros::ok() && !buzz_utility::buzz_script_done())
{
// Publish topics
neighbours_pos_publisher();
if(statepub_active) uavstate_publisher();
state_publisher();
grid_publisher();
send_MPpayload();
// Check updater state and step code
@ -184,10 +175,6 @@ void roscontroller::RosControllerRun()
<< cur_pos.altitude * 100000 << ",";
log << (int)no_of_robots<<",";
log << neighbours_pos_map.size()<< ",";
log<<(int)inmsgdata.size()<<",";
log<< message_number<<",";
log<< out_msg_time<<",";
log <<buzz_utility::getuavstate();
// if(neighbours_pos_map.size() > 0)log<<",";
map<int, buzz_utility::Pos_struct>::iterator it =
neighbours_pos_map.begin();
@ -202,7 +189,9 @@ void roscontroller::RosControllerRun()
<<","<<it->received_time;
}
inmsgdata.clear();
log<<std::endl;
log<<(int)inmsgdata.size()<<","<< message_number<<",";
log<< out_msg_time<<",";
log <<buzz_utility::get_bvmstate()<<std::endl;
// time_sync_step();
// if(debug)
@ -227,7 +216,7 @@ void roscontroller::RosControllerRun()
loop_rate.cycleTime().toSec());
// Safety land if the data coming from the flight controller are too old
if (fcu_timeout <= 0)
buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND);
buzzuav_closures::rc_call(NAV_LAND);
else
fcu_timeout -= 1 / BUZZRATE;
timer_step += 1;
@ -252,6 +241,14 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c)
ROS_ERROR("Provide a .bzz file to run in Launch file");
system("rosnode kill rosbuzz_node");
}
// Obtain debug mode from launch file parameter
if (n_c.getParam("debug", debug))
;
else
{
ROS_ERROR("Provide a debug mode in Launch file");
system("rosnode kill rosbuzz_node");
}
// Obtain rc service option from parameter server
if (n_c.getParam("rcclient", rcclient))
{
@ -335,7 +332,7 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle)
m_smTopic_infos.insert(pair<std::string, std::string>(topic, "sensor_msgs/LaserScan"));
node_handle.getParam("topics/battery", topic);
m_smTopic_infos.insert(pair<std::string, std::string>(topic, "mavros_msgs/BatteryStatus"));
m_smTopic_infos.insert(pair<std::string, std::string>(topic, "sensor_msgs/BatteryState"));
node_handle.getParam("topics/status", topic);
m_smTopic_infos.insert(pair<std::string, std::string>(topic, "mavros_msgs/State"));
@ -398,7 +395,7 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c)
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 5);
MPpayload_pub = n_c.advertise<mavros_msgs::Mavlink>("fleet_status", 5);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", MAX_NUMBER_OF_ROBOTS);
uavstate_pub = n_c.advertise<std_msgs::String>("uavstate", 5);
bvmstate_pub = n_c.advertise<std_msgs::String>("bvmstate", 5);
grid_pub = n_c.advertise<nav_msgs::OccupancyGrid>("grid", 5);
localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5);
@ -431,7 +428,7 @@ void roscontroller::Subscribe(ros::NodeHandle& n_c)
{
flight_status_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_status_update, this);
}
else if (it->second == "mavros_msgs/BatteryStatus")
else if (it->second == "sensor_msgs/BatteryState")
{
battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this);
}
@ -506,14 +503,14 @@ void roscontroller::neighbours_pos_publisher()
neigh_pos_pub.publish(neigh_pos_array);
}
void roscontroller::uavstate_publisher()
void roscontroller::state_publisher()
/*
/ Publish current UAVState from Buzz script
/----------------------------------------------------*/
{
std_msgs::String uavstate_msg;
uavstate_msg.data = buzz_utility::getuavstate();
uavstate_pub.publish(uavstate_msg);
std_msgs::String state_msg;
state_msg.data = buzz_utility::get_bvmstate();
bvmstate_pub.publish(state_msg);
}
void roscontroller::grid_publisher()
@ -639,7 +636,7 @@ with size......... | /
}
else{
// prepare rosbuzz msg header
tmphead[0] = (uint8_t)BUZZ_MESSAGE_WTO_TIME;
tmphead[0] = (uint8_t)BUZZ_MESSAGE_NO_TIME;
memcpy(rheader, tmphead, sizeof(uint64_t));
// push header into the buffer
payload_out.payload64.push_back(rheader[0]);
@ -714,7 +711,7 @@ script
float* gimbal;
switch (buzzuav_closures::bzz_cmd())
{
case buzzuav_closures::COMMAND_TAKEOFF:
case NAV_TAKEOFF:
goto_pos = buzzuav_closures::getgoto();
cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
@ -737,7 +734,7 @@ script
ROS_ERROR("Failed to call service from flight controller");
break;
case buzzuav_closures::COMMAND_LAND:
case NAV_LAND:
cmd_srv.request.command = buzzuav_closures::getcmd();
if (current_mode != "LAND")
{
@ -756,7 +753,7 @@ script
armstate = 0;
break;
case buzzuav_closures::COMMAND_GOHOME: // TODO: NOT FULLY IMPLEMENTED/TESTED !!!
case NAV_RETURN_TO_LAUNCH: // TODO: NOT FULLY IMPLEMENTED/TESTED !!!
cmd_srv.request.param5 = home.latitude;
cmd_srv.request.param6 = home.longitude;
cmd_srv.request.param7 = home.altitude;
@ -769,32 +766,7 @@ script
ROS_ERROR("Failed to call service from flight controller");
break;
case buzzuav_closures::COMMAND_GOTO: // TOOD: NOT FULLY IMPLEMENTED/TESTED !!!
goto_pos = buzzuav_closures::getgoto();
cmd_srv.request.param5 = goto_pos[0];
cmd_srv.request.param6 = goto_pos[1];
cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
if (mav_client.call(cmd_srv))
{
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
}
else
ROS_ERROR("Failed to call service from flight controller");
#ifdef MAVROSKINETIC
cmd_srv.request.command = mavros_msgs::CommandCode::MISSION_START;
#else
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
#endif
if (mav_client.call(cmd_srv))
{
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
}
else
ROS_ERROR("Failed to call service from flight controller");
break;
case buzzuav_closures::COMMAND_ARM:
case COMPONENT_ARM_DISARM:
if (!armstate)
{
SetMode("LOITER", 0);
@ -803,7 +775,7 @@ script
}
break;
case buzzuav_closures::COMMAND_DISARM:
case COMPONENT_ARM_DISARM+1:
if (armstate)
{
armstate = 0;
@ -812,22 +784,18 @@ script
}
break;
case buzzuav_closures::COMMAND_MOVETO:
case NAV_SPLINE_WAYPOINT:
goto_pos = buzzuav_closures::getgoto();
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
break;
case buzzuav_closures::COMMAND_GIMBAL:
case DO_MOUNT_CONTROL:
gimbal = buzzuav_closures::getgimbal();
cmd_srv.request.param1 = gimbal[0];
cmd_srv.request.param2 = gimbal[1];
cmd_srv.request.param3 = gimbal[2];
cmd_srv.request.param4 = gimbal[3];
#ifdef MAVROSKINETIC
cmd_srv.request.command = mavros_msgs::CommandCode::DO_MOUNT_CONTROL;
#else
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
#endif
cmd_srv.request.command = DO_MOUNT_CONTROL;
if (mav_client.call(cmd_srv))
{
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
@ -836,7 +804,7 @@ script
ROS_ERROR("Failed to call service from flight controller");
break;
case buzzuav_closures::COMMAND_PICTURE:
case IMAGE_START_CAPTURE:
ROS_INFO("TAKING A PICTURE HERE!! --------------");
mavros_msgs::CommandBool capture_command;
if (capture_srv.call(capture_command))
@ -902,10 +870,10 @@ float roscontroller::constrainAngle(float x)
/ Wrap the angle between -pi, pi
----------------------------------------------------------- */
{
x = fmod(x, 2 * M_PI);
x = fmod(x + M_PI, 2 * M_PI);
if (x < 0.0)
x += 2 * M_PI;
return x;
return x - M_PI;
}
void roscontroller::gps_rb(POSE nei_pos, double out[])
@ -917,10 +885,7 @@ void roscontroller::gps_rb(POSE nei_pos, double out[])
float ned_x = 0.0, ned_y = 0.0;
gps_ned_cur(ned_x, ned_y, nei_pos);
out[0] = sqrt(ned_x * ned_x + ned_y * ned_y);
// out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = atan2(ned_y, ned_x);
out[1] = constrainAngle(atan2(ned_y, ned_x));
// out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = 0.0;
}
@ -996,6 +961,7 @@ void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPt
{
cur_pos.x = msg->pose.position.x;
cur_pos.y = msg->pose.position.y;
buzzuav_closures::set_currentNEDpos(msg->pose.position.y,msg->pose.position.x);
// cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead
tf::Quaternion q(
@ -1156,7 +1122,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
}
// BVM FIFO message
else if ((int)mtype == (int)BUZZ_MESSAGE_TIME ||
(int)mtype == (int)BUZZ_MESSAGE_WTO_TIME)
(int)mtype == (int)BUZZ_MESSAGE_NO_TIME)
{
uint64_t message_obt[msg->payload64.size() - 1];
// Go throught the obtained payload
@ -1217,25 +1183,20 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m
int rc_cmd;
switch (req.command)
{
case mavros_msgs::CommandCode::NAV_TAKEOFF:
case NAV_TAKEOFF:
ROS_INFO("RC_call: TAKE OFF!!!!");
rc_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF;
rc_cmd = NAV_TAKEOFF;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_LAND:
case NAV_LAND:
ROS_INFO("RC_Call: LAND!!!!");
rc_cmd = mavros_msgs::CommandCode::NAV_LAND;
rc_cmd = NAV_LAND;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
#ifdef MAVROSKINETIC
case mavros_msgs::CommandCode::COMPONENT_ARM_DISARM:
rc_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM;
#else
case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM:
rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
#endif
case COMPONENT_ARM_DISARM:
rc_cmd = COMPONENT_ARM_DISARM;
armstate = req.param1;
if (armstate)
{
@ -1250,31 +1211,23 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m
res.success = true;
}
break;
case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
case NAV_RETURN_TO_LAUNCH:
ROS_INFO("RC_Call: GO HOME!!!!");
rc_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
rc_cmd = NAV_RETURN_TO_LAUNCH;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_WAYPOINT:
case NAV_WAYPOINT:
ROS_INFO("RC_Call: GO TO!!!! ");
buzzuav_closures::rc_set_goto(req.param1, req.param5, req.param6, req.param7);
rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT;
rc_cmd = NAV_WAYPOINT;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
#ifdef MAVROSKINETIC
case mavros_msgs::CommandCode::DO_MOUNT_CONTROL:
#else
case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL:
#endif
case DO_MOUNT_CONTROL:
ROS_INFO("RC_Call: Gimbal!!!! ");
buzzuav_closures::rc_set_gimbal(req.param1, req.param2, req.param3, req.param4, req.param5);
#ifdef MAVROSKINETIC
rc_cmd = mavros_msgs::CommandCode::DO_MOUNT_CONTROL;
#else
rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
#endif
rc_cmd = DO_MOUNT_CONTROL;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
@ -1303,7 +1256,7 @@ void roscontroller::get_number_of_robots()
/ Garbage collector for the number of robots in the swarm
--------------------------------------------------------------------------*/
{
int cur_robots = (int)buzzdict_size(buzz_utility::get_vm()->swarmmembers) + 1;
int cur_robots = buzz_utility::get_swarmsize();
if (no_of_robots == 0)
{
no_of_robots = cur_robots;