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

fix subscriber format, added NED yaw, changed position to absolute_position and fixed proximity obstacles detection

This commit is contained in:
dave 2017-12-21 12:20:26 -05:00
parent 1f2fae047a
commit ceeee9f76e
7 changed files with 131 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
buzz_scripts/include/rrtstar.bzz
View File

@ -7,7 +7,7 @@ include "mapmatrix.bzz"
RRT_TIMEOUT = 500 RRT_TIMEOUT = 500
RRT_RUNSTEP = 3 RRT_RUNSTEP = 3
PROX_SENSOR_THRESHOLD = 0.11 PROX_SENSOR_THRESHOLD_M = 10
GSCALE = {.x=1, .y=1} GSCALE = {.x=1, .y=1}
map = nil map = nil
cur_cell = {} cur_cell = {}
@ -77,7 +77,7 @@ function getcell(m_curpos) {
} }
function populateGrid(m_pos) { function populateGrid(m_pos) {
#getproxobs(m_pos) getproxobs(m_pos)
getneiobs (m_pos) getneiobs (m_pos)
} }
@ -131,19 +131,19 @@ function getneiobs (m_curpos) {
# populate a line in front of the sensor using plateform independant proximity sensing # populate a line in front of the sensor using plateform independant proximity sensing
# TODO: adapt to M100 stereo # TODO: adapt to M100 stereo
function getproxobs (m_curpos) { function getproxobs (m_curpos) {
var foundobstacle = 0 var updated_obstacle = 0
cur_cell = getcell(m_curpos) cur_cell = getcell(m_curpos)
reduce(proximity, reduce(proximity,
function(key, value, acc) { function(key, value, acc) {
obs = math.vec2.add(math.vec2.newp(1.15, value.angle * math.pi / 180.0 + absolute_position.theta), cur_cell) obs = getcell(math.vec2.newp(value.value, value.angle + absolute_position.yaw))
obs2 = math.vec2.add(math.vec2.newp(2.15, value.angle * math.pi / 180.0 + absolute_position.theta), cur_cell) obs2 = getcell(math.vec2.newp(value.value + 1.0, value.angle + absolute_position.yaw))
per = math.vec2.sub(obs,cur_cell) per = math.vec2.sub(obs,cur_cell)
obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs) obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs)
obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2) obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2)
obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs) obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs)
obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2) obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2)
if(value.value > PROX_SENSOR_THRESHOLD) { if(value.value < PROX_SENSOR_THRESHOLD_M) {
if(map[math.round(obs.x)][math.round(obs.y)]!=0) { if(map[math.round(obs.x)][math.round(obs.y)]!=0) {
add_obstacle(obs, 0, 0.25) add_obstacle(obs, 0, 0.25)
add_obstacle(obs2, 0, 0.25) add_obstacle(obs2, 0, 0.25)
@ -151,16 +151,16 @@ function getproxobs (m_curpos) {
add_obstacle(obsr2, 0, 0.25) add_obstacle(obsr2, 0, 0.25)
add_obstacle(obsl, 0, 0.25) add_obstacle(obsl, 0, 0.25)
add_obstacle(obsl2, 0, 0.25) add_obstacle(obsl2, 0, 0.25)
foundobstacle = 1 updated_obstacle = 1
} }
} else if(map[math.round(obs.x)][math.round(obs.y)]!=1) { } else if(map[math.round(obs.x)][math.round(obs.y)]!=1) {
remove_obstacle(obs, 0, 0.05) remove_obstacle(obs, 0, 0.05)
foundobstacle = 1 updated_obstacle = 1
} }
return acc return acc
}, math.vec2.new(0, 0)) }, math.vec2.new(0, 0))
return foundobstacle return updated_obstacle
} }
# check if any obstacle blocks the way # check if any obstacle blocks the way

32
buzz_scripts/include/uavstates.bzz
View File

@ -34,13 +34,13 @@ function idle() {
function takeoff() { function takeoff() {
BVMSTATE = "TAKEOFF" BVMSTATE = "TAKEOFF"
statef=takeoff statef=takeoff
homegps = {.lat=position.latitude, .long=position.longitude} homegps = {.lat=absolute_position.latitude, .long=absolute_position.longitude}
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { if( flight.status == 2 and absolute_position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS, action, land, -1) barrier_set(ROBOTS, action, land, -1)
barrier_ready() barrier_ready()
} else { } else {
log("Altitude: ", position.altitude) log("Altitude: ", absolute_position.altitude)
neighbors.broadcast("cmd", 22) neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE) uav_takeoff(TARGET_ALTITUDE)
} }
@ -105,8 +105,8 @@ function gotoWPRRT(transf) {
# create the map # create the map
if(map==nil) { if(map==nil) {
dyn_init_map(rc_goal) dyn_init_map(rc_goal)
homegps.lat = position.latitude homegps.lat = absolute_position.latitude
homegps.long = position.longitude homegps.long = absolute_position.longitude
} }
if(Path==nil) { if(Path==nil) {
@ -122,7 +122,7 @@ function gotoWPRRT(transf) {
if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) { if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) {
populateGrid(m_pos) populateGrid(m_pos)
if(check_path(Path, cur_goal_l, m_pos, 0)) { if(check_path(Path, cur_goal_l, m_pos, 0)) {
uav_moveto(0.0, 0.0, rc_goto.altitude-position.altitude) uav_moveto(0.0, 0.0, rc_goto.altitude - absolute_position.altitude)
Path = nil Path = nil
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0)
# re-start the planner # re-start the planner
@ -130,7 +130,7 @@ function gotoWPRRT(transf) {
cur_goal_l = 1 cur_goal_l = 1
} else { } else {
cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt)) cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt))
uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude-position.altitude) uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude - absolute_position.altitude)
} }
} else } else
cur_goal_l = cur_goal_l + 1 cur_goal_l = cur_goal_l + 1
@ -148,11 +148,11 @@ function gotoWP(transf) {
log("Sorry this is too far.") log("Sorry this is too far.")
else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - absolute_position.altitude)
} else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
transf() transf()
else else
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - absolute_position.altitude)
} }
function follow() { function follow() {
@ -260,8 +260,8 @@ function LimitAngle(angle){
} }
function vec_from_gps(lat, lon, home_ref) { function vec_from_gps(lat, lon, home_ref) {
d_lon = lon - position.longitude d_lon = lon - absolute_position.longitude
d_lat = lat - position.latitude d_lat = lat - absolute_position.latitude
if(home_ref == 1) { if(home_ref == 1) {
d_lon = lon - homegps.long d_lon = lon - homegps.long
d_lat = lat - homegps.lat d_lat = lat - homegps.lat
@ -277,17 +277,17 @@ function gps_from_vec(vec) {
Lgoal = {.latitude=0, .longitude=0} Lgoal = {.latitude=0, .longitude=0}
Vrange = math.vec2.length(vec) Vrange = math.vec2.length(vec)
Vbearing = LimitAngle(math.atan(vec.y, vec.x)) Vbearing = LimitAngle(math.atan(vec.y, vec.x))
# print("rb2gps: ",Vrange,Vbearing,position.latitude,position.longitude) # print("rb2gps: ",Vrange,Vbearing, absolute_position.latitude, absolute_position.longitude)
latR = position.latitude*math.pi/180.0; latR = absolute_position.latitude*math.pi/180.0;
lonR = position.longitude*math.pi/180.0; lonR = absolute_position.longitude*math.pi/180.0;
target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing)); target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing));
target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat)); target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat));
Lgoal.latitude = target_lat*180.0/math.pi; Lgoal.latitude = target_lat*180.0/math.pi;
Lgoal.longitude = target_lon*180.0/math.pi; Lgoal.longitude = target_lon*180.0/math.pi;
#d_lat = (vec.x / 6371000.0)*180.0/math.pi; #d_lat = (vec.x / 6371000.0)*180.0/math.pi;
#goal.latitude = d_lat + position.latitude; #goal.latitude = d_lat + absolute_position.latitude;
#d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi; #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi;
#goal.longitude = d_lon + position.longitude; #goal.longitude = d_lon + absolute_position.longitude;
return Lgoal return Lgoal
} }

24
buzz_scripts/testRRT.bzz
View File

@ -5,7 +5,7 @@ include "vstigenv.bzz"
function action() { function action() {
statef=action statef=action
uav_storegoal(45.5088103899,-73.1540826153,25.0) uav_storegoal(45.5088103899,-73.1540826153,5.0)
set_goto(idle) set_goto(idle)
} }
@ -13,10 +13,12 @@ function action() {
function init() { function init() {
uav_initstig() uav_initstig()
uav_initswarm() uav_initswarm()
statef=turnedoff
BVMSTATE = "TURNEDOFF" TARGET_ALTITUDE = 5.0 # m.
#statef = takeoff # statef=turnedoff
#BVMSTATE = "TAKEOFF" # BVMSTATE = "TURNEDOFF"
statef = takeoff
BVMSTATE = "TAKEOFF"
} }
# Executed at each time step. # Executed at each time step.
@ -26,12 +28,12 @@ function step() {
statef() statef()
log("Current state: ", BVMSTATE) log("Current state: ", BVMSTATE)
log("Obstacles: ") # log("Obstacles: ")
reduce(proximity, # reduce(proximity,
function(key, value, acc) { # function(key, value, acc) {
log(key, " - ", value.angle, value.value) # log(key, " - ", value.angle, value.value)
return acc # return acc
}, math.vec2.new(0, 0)) # }, math.vec2.new(0, 0))
} }
# Executed once when the robot (or the simulator) is reset. # Executed once when the robot (or the simulator) is reset.

2
include/buzzuav_closures.h
View File

@ -89,7 +89,7 @@ void set_filtered_packet_loss(float value);
/* /*
* sets current position * sets current position
*/ */
void set_currentpos(double latitude, double longitude, double altitude); void set_currentpos(double latitude, double longitude, float altitude, float yaw);
/* /*
* returns the current go to position * returns the current go to position
*/ */

56
include/roscontroller.h
View File

@ -1,5 +1,6 @@
#pragma once #pragma once
#include <ros/ros.h> #include <ros/ros.h>
#include <tf/tf.h>
#include <sensor_msgs/NavSatFix.h> #include <sensor_msgs/NavSatFix.h>
#include <nav_msgs/OccupancyGrid.h> #include <nav_msgs/OccupancyGrid.h>
#include <std_msgs/UInt8.h> #include <std_msgs/UInt8.h>
@ -51,7 +52,6 @@ class roscontroller
public: public:
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv); roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
~roscontroller(); ~roscontroller();
// void RosControllerInit();
void RosControllerRun(); void RosControllerRun();
static const string CAPTURE_SRV_DEFAULT_NAME; static const string CAPTURE_SRV_DEFAULT_NAME;
@ -70,25 +70,27 @@ private:
Num_robot_count count_robots; Num_robot_count count_robots;
struct gps struct POSE
{ {
double longitude = 0.0; double longitude = 0.0;
double latitude = 0.0; double latitude = 0.0;
float altitude = 0.0; float altitude = 0.0;
// NED coordinates
float x = 0.0;
float y = 0.0;
float z = 0.0;
float yaw = 0.0;
}; };
typedef struct gps GPS; typedef struct POSE ros_pose;
GPS target, home, cur_pos; ros_pose target, home, cur_pos;
double cur_rel_altitude;
uint64_t payload; uint64_t payload;
std::map<int, buzz_utility::Pos_struct> neighbours_pos_map; std::map<int, buzz_utility::Pos_struct> neighbours_pos_map;
std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map; std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
// std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
int timer_step = 0; int timer_step = 0;
int robot_id = 0; int robot_id = 0;
std::string robot_name = ""; std::string robot_name = "";
// int oldcmdID=0;
int rc_cmd; int rc_cmd;
float fcu_timeout; float fcu_timeout;
@ -96,7 +98,9 @@ private:
int barrier; int barrier;
int message_number = 0; int message_number = 0;
uint8_t no_of_robots = 0; uint8_t no_of_robots = 0;
/*tmp to be corrected*/ bool rcclient;
bool xbeeplugged = false;
bool multi_msg;
uint8_t no_cnt = 0; uint8_t no_cnt = 0;
uint8_t old_val = 0; uint8_t old_val = 0;
std::string bzzfile_name; std::string bzzfile_name;
@ -107,20 +111,18 @@ private:
std::string bcfname, dbgfname; std::string bcfname, dbgfname;
std::string out_payload; std::string out_payload;
std::string in_payload; std::string in_payload;
std::string obstacles_topic;
std::string stand_by; std::string stand_by;
std::string xbeesrv_name; std::string xbeesrv_name;
std::string capture_srv_name; std::string capture_srv_name;
std::string setpoint_name; std::string setpoint_name;
std::string stream_client_name; std::string stream_client_name;
std::string relative_altitude_sub_name;
std::string setpoint_nonraw; std::string setpoint_nonraw;
bool rcclient;
bool xbeeplugged = false; // ROS service, publishers and subscribers
bool multi_msg;
ros::ServiceClient mav_client; ros::ServiceClient mav_client;
ros::ServiceClient xbeestatus_srv; ros::ServiceClient xbeestatus_srv;
ros::ServiceClient capture_srv; ros::ServiceClient capture_srv;
ros::ServiceClient stream_client;
ros::Publisher payload_pub; ros::Publisher payload_pub;
ros::Publisher MPpayload_pub; ros::Publisher MPpayload_pub;
ros::Publisher neigh_pos_pub; ros::Publisher neigh_pos_pub;
@ -137,23 +139,16 @@ private:
ros::Subscriber obstacle_sub; ros::Subscriber obstacle_sub;
ros::Subscriber Robot_id_sub; ros::Subscriber Robot_id_sub;
ros::Subscriber relative_altitude_sub; ros::Subscriber relative_altitude_sub;
std::string local_pos_sub_name;
ros::Subscriber local_pos_sub; ros::Subscriber local_pos_sub;
double local_pos_new[3];
ros::ServiceClient stream_client; std::map<std::string, std::string> m_smTopic_infos;
int setpoint_counter; int setpoint_counter;
double my_x = 0, my_y = 0;
std::ofstream log; std::ofstream log;
/*Commands for flight controller*/ // Commands for flight controller
// mavros_msgs::CommandInt cmd_srv;
mavros_msgs::CommandLong cmd_srv; mavros_msgs::CommandLong cmd_srv;
std::vector<std::string> m_sMySubscriptions;
std::map<std::string, std::string> m_smTopic_infos;
mavros_msgs::CommandBool m_cmdBool; mavros_msgs::CommandBool m_cmdBool;
ros::ServiceClient arm_client; ros::ServiceClient arm_client;
@ -161,7 +156,7 @@ private:
mavros_msgs::SetMode m_cmdSetMode; mavros_msgs::SetMode m_cmdSetMode;
ros::ServiceClient mode_client; ros::ServiceClient mode_client;
/*Initialize publisher and subscriber, done in the constructor*/ // Initialize publisher and subscriber, done in the constructor
void Initialize_pub_sub(ros::NodeHandle& n_c); void Initialize_pub_sub(ros::NodeHandle& n_c);
std::string current_mode; std::string current_mode;
@ -204,8 +199,8 @@ private:
/*convert from spherical to cartesian coordinate system callback */ /*convert from spherical to cartesian coordinate system callback */
float constrainAngle(float x); float constrainAngle(float x);
void gps_rb(GPS nei_pos, double out[]); void gps_rb(POSE nei_pos, double out[]);
void gps_ned_cur(float& ned_x, float& ned_y, GPS t); void gps_ned_cur(float& ned_x, float& ned_y, POSE t);
void gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon, void gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon,
double gps_r_lat); double gps_r_lat);
@ -219,10 +214,10 @@ private:
void flight_status_update(const mavros_msgs::State::ConstPtr& msg); void flight_status_update(const mavros_msgs::State::ConstPtr& msg);
/*current position callback*/ /*current position callback*/
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); void global_gps_callback(const sensor_msgs::NavSatFix::ConstPtr& msg);
/*current relative altitude callback*/ /*current relative altitude callback*/
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg); void rel_alt_callback(const std_msgs::Float64::ConstPtr& msg);
/*payload callback callback*/ /*payload callback callback*/
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg); void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
@ -234,7 +229,7 @@ private:
void set_robot_id(const std_msgs::UInt8::ConstPtr& msg); void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
/*Obstacle distance table callback*/ /*Obstacle distance table callback*/
void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg); void obstacle_dist_callback(const sensor_msgs::LaserScan::ConstPtr& msg);
/*Get publisher and subscriber from YML file*/ /*Get publisher and subscriber from YML file*/
void GetSubscriptionParameters(ros::NodeHandle& node_handle); void GetSubscriptionParameters(ros::NodeHandle& node_handle);
@ -250,8 +245,6 @@ private:
void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose); void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose);
// void WaypointMissionSetup(float lat, float lng, float alt);
void fc_command_setup(); void fc_command_setup();
void SetLocalPosition(float x, float y, float z, float yaw); void SetLocalPosition(float x, float y, float z, float yaw);
@ -262,6 +255,7 @@ private:
void get_number_of_robots(); void get_number_of_robots();
// functions related to Xbee modules information
void GetRobotId(); void GetRobotId();
bool GetDequeFull(bool& result); bool GetDequeFull(bool& result);
bool GetRssi(float& result); bool GetRssi(float& result);
@ -269,7 +263,7 @@ private:
bool GetAPIRssi(const uint8_t short_id, float& result); bool GetAPIRssi(const uint8_t short_id, float& result);
bool GetRawPacketLoss(const uint8_t short_id, float& result); bool GetRawPacketLoss(const uint8_t short_id, float& result);
bool GetFilteredPacketLoss(const uint8_t short_id, float& result); bool GetFilteredPacketLoss(const uint8_t short_id, float& result);
void get_xbee_status(); void get_xbee_status();
}; };
} }

11
src/buzzuav_closures.cpp
View File

@ -18,7 +18,7 @@ static double rc_goto_pos[3];
static float rc_gimbal[4]; static float rc_gimbal[4];
static float batt[3]; static float batt[3];
static float obst[5] = { 0, 0, 0, 0, 0 }; static float obst[5] = { 0, 0, 0, 0, 0 };
static double cur_pos[3]; static double cur_pos[4];
static uint8_t status; static uint8_t status;
static int cur_cmd = 0; static int cur_cmd = 0;
static int rc_cmd = 0; static int rc_cmd = 0;
@ -637,7 +637,7 @@ int buzzuav_update_xbee_status(buzzvm_t vm)
return vm->state; return vm->state;
} }
void set_currentpos(double latitude, double longitude, double altitude) void set_currentpos(double latitude, double longitude, float altitude, float yaw)
/* /*
/ update interface position array / update interface position array
-----------------------------------*/ -----------------------------------*/
@ -645,6 +645,7 @@ void set_currentpos(double latitude, double longitude, double altitude)
cur_pos[0] = latitude; cur_pos[0] = latitude;
cur_pos[1] = longitude; cur_pos[1] = longitude;
cur_pos[2] = altitude; cur_pos[2] = altitude;
cur_pos[3] = yaw;
} }
// adds neighbours position // adds neighbours position
void neighbour_pos_callback(int id, float range, float bearing, float elevation) void neighbour_pos_callback(int id, float range, float bearing, float elevation)
@ -677,7 +678,7 @@ int buzzuav_update_currentpos(buzzvm_t vm)
/ Update the BVM position table / Update the BVM position table
/------------------------------------------------------*/ /------------------------------------------------------*/
{ {
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "absolute_position", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
@ -691,6 +692,10 @@ int buzzuav_update_currentpos(buzzvm_t vm)
buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1));
buzzvm_pushf(vm, cur_pos[2]); buzzvm_pushf(vm, cur_pos[2]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "yaw", 1));
buzzvm_pushf(vm, cur_pos[3]);
buzzvm_tput(vm);
buzzvm_gstore(vm); buzzvm_gstore(vm);
return vm->state; return vm->state;
} }

108
src/roscontroller.cpp
View File

@ -249,30 +249,37 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle)
/Obtains publisher, subscriber and services from yml config file /Obtains publisher, subscriber and services from yml config file
/-----------------------------------------------------------------------------------*/ /-----------------------------------------------------------------------------------*/
{ {
m_sMySubscriptions.clear(); std::string topic;
std::string gps_topic; if (node_handle.getParam("topics/gps", topic))
if (node_handle.getParam("topics/gps", gps_topic))
; ;
else else
{ {
ROS_ERROR("Provide a gps topic in Launch file"); ROS_ERROR("Provide a gps topic in YAML file");
system("rosnode kill rosbuzz_node"); system("rosnode kill rosbuzz_node");
} }
m_smTopic_infos.insert(pair<std::string, std::string>(gps_topic, "sensor_msgs/NavSatFix")); m_smTopic_infos.insert(pair<std::string, std::string>(topic, "sensor_msgs/NavSatFix"));
if (node_handle.getParam("topics/localpos", topic))
;
else
{
ROS_ERROR("Provide a localpos name in YAML file");
system("rosnode kill rosbuzz_node");
}
m_smTopic_infos.insert(pair<std::string, std::string>(topic, "geometry_msgs/PoseStamped"));
std::string battery_topic; node_handle.getParam("topics/obstacles", topic);
node_handle.getParam("topics/battery", battery_topic); m_smTopic_infos.insert(pair<std::string, std::string>(topic, "sensor_msgs/LaserScan"));
m_smTopic_infos.insert(pair<std::string, std::string>(battery_topic, "mavros_msgs/BatteryStatus"));
std::string status_topic; node_handle.getParam("topics/battery", topic);
node_handle.getParam("topics/status", status_topic); m_smTopic_infos.insert(pair<std::string, std::string>(topic, "mavros_msgs/BatteryStatus"));
m_smTopic_infos.insert(pair<std::string, std::string>(status_topic, "mavros_msgs/State"));
node_handle.getParam("topics/estatus", status_topic);
m_smTopic_infos.insert(pair<std::string, std::string>(status_topic, "mavros_msgs/ExtendedState"));
std::string altitude_topic; node_handle.getParam("topics/status", topic);
node_handle.getParam("topics/altitude", altitude_topic); m_smTopic_infos.insert(pair<std::string, std::string>(topic, "mavros_msgs/State"));
m_smTopic_infos.insert(pair<std::string, std::string>(altitude_topic, "std_msgs/Float64")); node_handle.getParam("topics/estatus", topic);
m_smTopic_infos.insert(pair<std::string, std::string>(topic, "mavros_msgs/ExtendedState"));
node_handle.getParam("topics/altitude", topic);
m_smTopic_infos.insert(pair<std::string, std::string>(topic, "std_msgs/Float64"));
// Obtain required topic and service names from the parameter server // Obtain required topic and service names from the parameter server
if (node_handle.getParam("topics/fcclient", fcclient_name)) if (node_handle.getParam("topics/fcclient", fcclient_name))
@ -310,15 +317,6 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle)
ROS_ERROR("Provide a mode client name in Launch file"); ROS_ERROR("Provide a mode client name in Launch file");
system("rosnode kill rosbuzz_node"); system("rosnode kill rosbuzz_node");
} }
if (node_handle.getParam("topics/localpos", local_pos_sub_name))
;
else
{
ROS_ERROR("Provide a localpos name in YAML file");
system("rosnode kill rosbuzz_node");
}
node_handle.getParam("topics/obstacles", obstacles_topic);
} }
void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c)
@ -332,8 +330,6 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c)
payload_sub = n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this); payload_sub = n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this);
obstacle_sub = n_c.subscribe(obstacles_topic, 5, &roscontroller::obstacle_dist, this);
// publishers // publishers
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 5); payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 5);
MPpayload_pub = n_c.advertise<mavros_msgs::Mavlink>("fleet_status", 5); MPpayload_pub = n_c.advertise<mavros_msgs::Mavlink>("fleet_status", 5);
@ -353,8 +349,6 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c)
capture_srv = n_c.serviceClient<mavros_msgs::CommandBool>(capture_srv_name); capture_srv = n_c.serviceClient<mavros_msgs::CommandBool>(capture_srv_name);
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name); stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
local_pos_sub = n_c.subscribe(local_pos_sub_name, 5, &roscontroller::local_pos_callback, this);
multi_msg = true; multi_msg = true;
} }
@ -379,11 +373,19 @@ void roscontroller::Subscribe(ros::NodeHandle& n_c)
} }
else if (it->second == "sensor_msgs/NavSatFix") else if (it->second == "sensor_msgs/NavSatFix")
{ {
current_position_sub = n_c.subscribe(it->first, 5, &roscontroller::current_pos, this); current_position_sub = n_c.subscribe(it->first, 5, &roscontroller::global_gps_callback, this);
} }
else if (it->second == "std_msgs/Float64") else if (it->second == "std_msgs/Float64")
{ {
relative_altitude_sub = n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this); relative_altitude_sub = n_c.subscribe(it->first, 5, &roscontroller::rel_alt_callback, this);
}
else if (it->second == "geometry_msgs/PoseStamped")
{
local_pos_sub = n_c.subscribe(it->first, 5, &roscontroller::local_pos_callback, this);
}
else if (it->second == "sensor_msgs/LaserScan")
{
obstacle_sub = n_c.subscribe(it->first, 5, &roscontroller::obstacle_dist_callback, this);
} }
std::cout << "Subscribed to: " << it->first << endl; std::cout << "Subscribed to: " << it->first << endl;
@ -798,7 +800,7 @@ float roscontroller::constrainAngle(float x)
return x; return x;
} }
void roscontroller::gps_rb(GPS nei_pos, double out[]) void roscontroller::gps_rb(POSE nei_pos, double out[])
/* /*
/ Compute Range and Bearing of a neighbor in a local reference frame / Compute Range and Bearing of a neighbor in a local reference frame
/ from GPS coordinates / from GPS coordinates
@ -814,7 +816,7 @@ void roscontroller::gps_rb(GPS nei_pos, double out[])
out[2] = 0.0; out[2] = 0.0;
} }
void roscontroller::gps_ned_cur(float& ned_x, float& ned_y, GPS t) void roscontroller::gps_ned_cur(float& ned_x, float& ned_y, POSE t)
/* /*
/ Get GPS from NED and a reference GPS point (struct input) / Get GPS from NED and a reference GPS point (struct input)
----------------------------------------------------------- */ ----------------------------------------------------------- */
@ -868,39 +870,47 @@ void roscontroller::flight_extended_status_update(const mavros_msgs::ExtendedSta
buzzuav_closures::flight_status_update(msg->landed_state); buzzuav_closures::flight_status_update(msg->landed_state);
} }
void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg) void roscontroller::global_gps_callback(const sensor_msgs::NavSatFix::ConstPtr& msg)
/* /*
/ Update current GPS position into BVM from subscriber / Update current GPS position into BVM from subscriber
/-------------------------------------------------------------*/ /-------------------------------------------------------------*/
{ {
// DEBUG // reset timeout counter
// ROS_INFO("Altitude out: %f", cur_rel_altitude);
fcu_timeout = TIMEOUT; fcu_timeout = TIMEOUT;
set_cur_pos(msg->latitude, msg->longitude, cur_rel_altitude); // msg->altitude); set_cur_pos(msg->latitude, msg->longitude, cur_pos.z);
buzzuav_closures::set_currentpos(msg->latitude, msg->longitude, cur_rel_altitude); // msg->altitude); buzzuav_closures::set_currentpos(msg->latitude, msg->longitude, cur_pos.z, cur_pos.yaw);
} }
void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose) void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
/* /*
/ Update current position for flight controller NED offset / Update current position for flight controller NED offset
/-------------------------------------------------------------*/ /-------------------------------------------------------------*/
{ {
local_pos_new[0] = pose->pose.position.x; cur_pos.x = msg->pose.position.x;
local_pos_new[1] = pose->pose.position.y; cur_pos.y = msg->pose.position.y;
local_pos_new[2] = pose->pose.position.z; // cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead
tf::Quaternion q(
msg->pose.orientation.x,
msg->pose.orientation.y,
msg->pose.orientation.z,
msg->pose.orientation.w);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
cur_pos.yaw = yaw;
} }
void roscontroller::current_rel_alt(const std_msgs::Float64::ConstPtr& msg) void roscontroller::rel_alt_callback(const std_msgs::Float64::ConstPtr& msg)
/* /*
/ Update altitude into BVM from subscriber / Update altitude into BVM from subscriber
/-------------------------------------------------------------*/ /-------------------------------------------------------------*/
{ {
// DEBUG // DEBUG
// ROS_INFO("Altitude in: %f", msg->data); // ROS_INFO("Altitude in: %f", msg->data);
cur_rel_altitude = (double)msg->data; cur_pos.z = (double)msg->data;
} }
void roscontroller::obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg) void roscontroller::obstacle_dist_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
/* /*
/Set obstacle Obstacle distance table into BVM from subscriber /Set obstacle Obstacle distance table into BVM from subscriber
/-------------------------------------------------------------*/ /-------------------------------------------------------------*/
@ -925,9 +935,9 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw)
moveMsg.header.frame_id = 1; moveMsg.header.frame_id = 1;
// DEBUG // DEBUG
// ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f", local_pos_new[0], local_pos_new[1], x, y); // ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f", cur_pos.x, cur_pos.y, x, y);
moveMsg.pose.position.x = local_pos_new[0] + y; moveMsg.pose.position.x = cur_pos.x + y;
moveMsg.pose.position.y = local_pos_new[1] + x; moveMsg.pose.position.y = cur_pos.y + x;
moveMsg.pose.position.z = z; moveMsg.pose.position.z = z;
moveMsg.pose.orientation.x = 0; moveMsg.pose.orientation.x = 0;
@ -1038,7 +1048,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
memcpy(neighbours_pos_payload, message_obt, 3 * sizeof(uint64_t)); memcpy(neighbours_pos_payload, message_obt, 3 * sizeof(uint64_t));
buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0], neighbours_pos_payload[1], buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0], neighbours_pos_payload[1],
neighbours_pos_payload[2]); neighbours_pos_payload[2]);
GPS nei_pos; POSE nei_pos;
nei_pos.latitude = neighbours_pos_payload[0]; nei_pos.latitude = neighbours_pos_payload[0];
nei_pos.longitude = neighbours_pos_payload[1]; nei_pos.longitude = neighbours_pos_payload[1];
nei_pos.altitude = neighbours_pos_payload[2]; nei_pos.altitude = neighbours_pos_payload[2];