fix subscriber format, added NED yaw, changed position to absolute_position and fixed proximity obstacles detection
This commit is contained in:
dave
parent
1f2fae047a
commit
ceeee9f76e
|
|
@ -7,7 +7,7 @@ include "mapmatrix.bzz"
|
|||
|
||||
RRT_TIMEOUT = 500
|
||||
RRT_RUNSTEP = 3
|
||||
PROX_SENSOR_THRESHOLD = 0.11
|
||||
PROX_SENSOR_THRESHOLD_M = 10
|
||||
GSCALE = {.x=1, .y=1}
|
||||
map = nil
|
||||
cur_cell = {}
|
||||
|
|
@ -77,7 +77,7 @@ function getcell(m_curpos) {
|
|||
}
|
||||
|
||||
function populateGrid(m_pos) {
|
||||
#getproxobs(m_pos)
|
||||
getproxobs(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
|
||||
# TODO: adapt to M100 stereo
|
||||
function getproxobs (m_curpos) {
|
||||
var foundobstacle = 0
|
||||
var updated_obstacle = 0
|
||||
cur_cell = getcell(m_curpos)
|
||||
|
||||
reduce(proximity,
|
||||
function(key, value, acc) {
|
||||
obs = math.vec2.add(math.vec2.newp(1.15, value.angle * math.pi / 180.0 + absolute_position.theta), cur_cell)
|
||||
obs2 = math.vec2.add(math.vec2.newp(2.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 = getcell(math.vec2.newp(value.value + 1.0, value.angle + absolute_position.yaw))
|
||||
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)
|
||||
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)
|
||||
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) {
|
||||
add_obstacle(obs, 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(obsl, 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) {
|
||||
remove_obstacle(obs, 0, 0.05)
|
||||
foundobstacle = 1
|
||||
updated_obstacle = 1
|
||||
}
|
||||
return acc
|
||||
}, math.vec2.new(0, 0))
|
||||
|
||||
return foundobstacle
|
||||
return updated_obstacle
|
||||
}
|
||||
|
||||
# check if any obstacle blocks the way
|
||||
|
|
|
|||
|
|
@ -34,13 +34,13 @@ function idle() {
|
|||
function takeoff() {
|
||||
BVMSTATE = "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_ready()
|
||||
} else {
|
||||
log("Altitude: ", position.altitude)
|
||||
log("Altitude: ", absolute_position.altitude)
|
||||
neighbors.broadcast("cmd", 22)
|
||||
uav_takeoff(TARGET_ALTITUDE)
|
||||
}
|
||||
|
|
@ -105,8 +105,8 @@ function gotoWPRRT(transf) {
|
|||
# create the map
|
||||
if(map==nil) {
|
||||
dyn_init_map(rc_goal)
|
||||
homegps.lat = position.latitude
|
||||
homegps.long = position.longitude
|
||||
homegps.lat = absolute_position.latitude
|
||||
homegps.long = absolute_position.longitude
|
||||
}
|
||||
|
||||
if(Path==nil) {
|
||||
|
|
@ -122,7 +122,7 @@ function gotoWPRRT(transf) {
|
|||
if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) {
|
||||
populateGrid(m_pos)
|
||||
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
|
||||
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0)
|
||||
# re-start the planner
|
||||
|
|
@ -130,7 +130,7 @@ function gotoWPRRT(transf) {
|
|||
cur_goal_l = 1
|
||||
} else {
|
||||
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
|
||||
cur_goal_l = cur_goal_l + 1
|
||||
|
|
@ -148,11 +148,11 @@ function gotoWP(transf) {
|
|||
log("Sorry this is too far.")
|
||||
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))
|
||||
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
|
||||
transf()
|
||||
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() {
|
||||
|
|
@ -260,8 +260,8 @@ function LimitAngle(angle){
|
|||
}
|
||||
|
||||
function vec_from_gps(lat, lon, home_ref) {
|
||||
d_lon = lon - position.longitude
|
||||
d_lat = lat - position.latitude
|
||||
d_lon = lon - absolute_position.longitude
|
||||
d_lat = lat - absolute_position.latitude
|
||||
if(home_ref == 1) {
|
||||
d_lon = lon - homegps.long
|
||||
d_lat = lat - homegps.lat
|
||||
|
|
@ -277,17 +277,17 @@ function gps_from_vec(vec) {
|
|||
Lgoal = {.latitude=0, .longitude=0}
|
||||
Vrange = math.vec2.length(vec)
|
||||
Vbearing = LimitAngle(math.atan(vec.y, vec.x))
|
||||
# print("rb2gps: ",Vrange,Vbearing,position.latitude,position.longitude)
|
||||
latR = position.latitude*math.pi/180.0;
|
||||
lonR = position.longitude*math.pi/180.0;
|
||||
# print("rb2gps: ",Vrange,Vbearing, absolute_position.latitude, absolute_position.longitude)
|
||||
latR = absolute_position.latitude*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_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.longitude = target_lon*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;
|
||||
#goal.longitude = d_lon + position.longitude;
|
||||
#goal.longitude = d_lon + absolute_position.longitude;
|
||||
|
||||
return Lgoal
|
||||
}
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@ include "vstigenv.bzz"
|
|||
|
||||
function action() {
|
||||
statef=action
|
||||
uav_storegoal(45.5088103899,-73.1540826153,25.0)
|
||||
uav_storegoal(45.5088103899,-73.1540826153,5.0)
|
||||
set_goto(idle)
|
||||
}
|
||||
|
||||
|
|
@ -13,10 +13,12 @@ function action() {
|
|||
function init() {
|
||||
uav_initstig()
|
||||
uav_initswarm()
|
||||
statef=turnedoff
|
||||
BVMSTATE = "TURNEDOFF"
|
||||
#statef = takeoff
|
||||
#BVMSTATE = "TAKEOFF"
|
||||
|
||||
TARGET_ALTITUDE = 5.0 # m.
|
||||
# statef=turnedoff
|
||||
# BVMSTATE = "TURNEDOFF"
|
||||
statef = takeoff
|
||||
BVMSTATE = "TAKEOFF"
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
|
|
@ -26,12 +28,12 @@ function step() {
|
|||
statef()
|
||||
|
||||
log("Current state: ", BVMSTATE)
|
||||
log("Obstacles: ")
|
||||
reduce(proximity,
|
||||
function(key, value, acc) {
|
||||
log(key, " - ", value.angle, value.value)
|
||||
return acc
|
||||
}, math.vec2.new(0, 0))
|
||||
# log("Obstacles: ")
|
||||
# reduce(proximity,
|
||||
# function(key, value, acc) {
|
||||
# log(key, " - ", value.angle, value.value)
|
||||
# return acc
|
||||
# }, math.vec2.new(0, 0))
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
|
|
|
|||
|
|
@ -89,7 +89,7 @@ void set_filtered_packet_loss(float value);
|
|||
/*
|
||||
* 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
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -1,5 +1,6 @@
|
|||
#pragma once
|
||||
#include <ros/ros.h>
|
||||
#include <tf/tf.h>
|
||||
#include <sensor_msgs/NavSatFix.h>
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
#include <std_msgs/UInt8.h>
|
||||
|
|
@ -51,7 +52,6 @@ class roscontroller
|
|||
public:
|
||||
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
|
||||
~roscontroller();
|
||||
// void RosControllerInit();
|
||||
void RosControllerRun();
|
||||
|
||||
static const string CAPTURE_SRV_DEFAULT_NAME;
|
||||
|
|
@ -70,25 +70,27 @@ private:
|
|||
|
||||
Num_robot_count count_robots;
|
||||
|
||||
struct gps
|
||||
struct POSE
|
||||
{
|
||||
double longitude = 0.0;
|
||||
double latitude = 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;
|
||||
double cur_rel_altitude;
|
||||
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;
|
||||
// std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
|
||||
int timer_step = 0;
|
||||
int robot_id = 0;
|
||||
std::string robot_name = "";
|
||||
// int oldcmdID=0;
|
||||
|
||||
int rc_cmd;
|
||||
float fcu_timeout;
|
||||
|
|
@ -96,7 +98,9 @@ private:
|
|||
int barrier;
|
||||
int message_number = 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 old_val = 0;
|
||||
std::string bzzfile_name;
|
||||
|
|
@ -107,20 +111,18 @@ private:
|
|||
std::string bcfname, dbgfname;
|
||||
std::string out_payload;
|
||||
std::string in_payload;
|
||||
std::string obstacles_topic;
|
||||
std::string stand_by;
|
||||
std::string xbeesrv_name;
|
||||
std::string capture_srv_name;
|
||||
std::string setpoint_name;
|
||||
std::string stream_client_name;
|
||||
std::string relative_altitude_sub_name;
|
||||
std::string setpoint_nonraw;
|
||||
bool rcclient;
|
||||
bool xbeeplugged = false;
|
||||
bool multi_msg;
|
||||
|
||||
// ROS service, publishers and subscribers
|
||||
ros::ServiceClient mav_client;
|
||||
ros::ServiceClient xbeestatus_srv;
|
||||
ros::ServiceClient capture_srv;
|
||||
ros::ServiceClient stream_client;
|
||||
ros::Publisher payload_pub;
|
||||
ros::Publisher MPpayload_pub;
|
||||
ros::Publisher neigh_pos_pub;
|
||||
|
|
@ -137,23 +139,16 @@ private:
|
|||
ros::Subscriber obstacle_sub;
|
||||
ros::Subscriber Robot_id_sub;
|
||||
ros::Subscriber relative_altitude_sub;
|
||||
|
||||
std::string local_pos_sub_name;
|
||||
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;
|
||||
double my_x = 0, my_y = 0;
|
||||
|
||||
std::ofstream log;
|
||||
|
||||
/*Commands for flight controller*/
|
||||
// mavros_msgs::CommandInt cmd_srv;
|
||||
// Commands for flight controller
|
||||
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;
|
||||
ros::ServiceClient arm_client;
|
||||
|
|
@ -161,7 +156,7 @@ private:
|
|||
mavros_msgs::SetMode m_cmdSetMode;
|
||||
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);
|
||||
|
||||
std::string current_mode;
|
||||
|
|
@ -204,8 +199,8 @@ private:
|
|||
|
||||
/*convert from spherical to cartesian coordinate system callback */
|
||||
float constrainAngle(float x);
|
||||
void gps_rb(GPS nei_pos, double out[]);
|
||||
void gps_ned_cur(float& ned_x, float& ned_y, GPS t);
|
||||
void gps_rb(POSE nei_pos, double out[]);
|
||||
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,
|
||||
double gps_r_lat);
|
||||
|
||||
|
|
@ -219,10 +214,10 @@ private:
|
|||
void flight_status_update(const mavros_msgs::State::ConstPtr& msg);
|
||||
|
||||
/*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*/
|
||||
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
|
||||
void rel_alt_callback(const std_msgs::Float64::ConstPtr& msg);
|
||||
|
||||
/*payload callback callback*/
|
||||
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
|
||||
|
|
@ -234,7 +229,7 @@ private:
|
|||
void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
|
||||
|
||||
/*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*/
|
||||
void GetSubscriptionParameters(ros::NodeHandle& node_handle);
|
||||
|
|
@ -250,8 +245,6 @@ private:
|
|||
|
||||
void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose);
|
||||
|
||||
// void WaypointMissionSetup(float lat, float lng, float alt);
|
||||
|
||||
void fc_command_setup();
|
||||
|
||||
void SetLocalPosition(float x, float y, float z, float yaw);
|
||||
|
|
@ -262,6 +255,7 @@ private:
|
|||
|
||||
void get_number_of_robots();
|
||||
|
||||
// functions related to Xbee modules information
|
||||
void GetRobotId();
|
||||
bool GetDequeFull(bool& result);
|
||||
bool GetRssi(float& result);
|
||||
|
|
@ -269,7 +263,7 @@ private:
|
|||
bool GetAPIRssi(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);
|
||||
|
||||
void get_xbee_status();
|
||||
|
||||
};
|
||||
}
|
||||
|
|
|
|||
|
|
@ -18,7 +18,7 @@ static double rc_goto_pos[3];
|
|||
static float rc_gimbal[4];
|
||||
static float batt[3];
|
||||
static float obst[5] = { 0, 0, 0, 0, 0 };
|
||||
static double cur_pos[3];
|
||||
static double cur_pos[4];
|
||||
static uint8_t status;
|
||||
static int cur_cmd = 0;
|
||||
static int rc_cmd = 0;
|
||||
|
|
@ -637,7 +637,7 @@ int buzzuav_update_xbee_status(buzzvm_t vm)
|
|||
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
|
||||
-----------------------------------*/
|
||||
|
|
@ -645,6 +645,7 @@ void set_currentpos(double latitude, double longitude, double altitude)
|
|||
cur_pos[0] = latitude;
|
||||
cur_pos[1] = longitude;
|
||||
cur_pos[2] = altitude;
|
||||
cur_pos[3] = yaw;
|
||||
}
|
||||
// adds neighbours position
|
||||
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
|
||||
/------------------------------------------------------*/
|
||||
{
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "absolute_position", 1));
|
||||
buzzvm_pusht(vm);
|
||||
buzzvm_dup(vm);
|
||||
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_pushf(vm, cur_pos[2]);
|
||||
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);
|
||||
return vm->state;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -249,30 +249,37 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle)
|
|||
/Obtains publisher, subscriber and services from yml config file
|
||||
/-----------------------------------------------------------------------------------*/
|
||||
{
|
||||
m_sMySubscriptions.clear();
|
||||
std::string gps_topic;
|
||||
if (node_handle.getParam("topics/gps", gps_topic))
|
||||
std::string topic;
|
||||
if (node_handle.getParam("topics/gps", topic))
|
||||
;
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Provide a gps topic in Launch file");
|
||||
ROS_ERROR("Provide a gps topic in YAML file");
|
||||
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/battery", battery_topic);
|
||||
m_smTopic_infos.insert(pair<std::string, std::string>(battery_topic, "mavros_msgs/BatteryStatus"));
|
||||
node_handle.getParam("topics/obstacles", topic);
|
||||
m_smTopic_infos.insert(pair<std::string, std::string>(topic, "sensor_msgs/LaserScan"));
|
||||
|
||||
std::string status_topic;
|
||||
node_handle.getParam("topics/status", status_topic);
|
||||
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"));
|
||||
node_handle.getParam("topics/battery", topic);
|
||||
m_smTopic_infos.insert(pair<std::string, std::string>(topic, "mavros_msgs/BatteryStatus"));
|
||||
|
||||
std::string altitude_topic;
|
||||
node_handle.getParam("topics/altitude", altitude_topic);
|
||||
m_smTopic_infos.insert(pair<std::string, std::string>(altitude_topic, "std_msgs/Float64"));
|
||||
node_handle.getParam("topics/status", topic);
|
||||
m_smTopic_infos.insert(pair<std::string, std::string>(topic, "mavros_msgs/State"));
|
||||
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
|
||||
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");
|
||||
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)
|
||||
|
|
@ -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);
|
||||
|
||||
obstacle_sub = n_c.subscribe(obstacles_topic, 5, &roscontroller::obstacle_dist, this);
|
||||
|
||||
// publishers
|
||||
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 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);
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
@ -379,11 +373,19 @@ void roscontroller::Subscribe(ros::NodeHandle& n_c)
|
|||
}
|
||||
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")
|
||||
{
|
||||
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;
|
||||
|
|
@ -798,7 +800,7 @@ float roscontroller::constrainAngle(float 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
|
||||
/ from GPS coordinates
|
||||
|
|
@ -814,7 +816,7 @@ void roscontroller::gps_rb(GPS nei_pos, double out[])
|
|||
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)
|
||||
----------------------------------------------------------- */
|
||||
|
|
@ -868,39 +870,47 @@ void roscontroller::flight_extended_status_update(const mavros_msgs::ExtendedSta
|
|||
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
|
||||
/-------------------------------------------------------------*/
|
||||
{
|
||||
// DEBUG
|
||||
// ROS_INFO("Altitude out: %f", cur_rel_altitude);
|
||||
// reset timeout counter
|
||||
fcu_timeout = TIMEOUT;
|
||||
set_cur_pos(msg->latitude, msg->longitude, cur_rel_altitude); // msg->altitude);
|
||||
buzzuav_closures::set_currentpos(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_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
|
||||
/-------------------------------------------------------------*/
|
||||
{
|
||||
local_pos_new[0] = pose->pose.position.x;
|
||||
local_pos_new[1] = pose->pose.position.y;
|
||||
local_pos_new[2] = pose->pose.position.z;
|
||||
cur_pos.x = msg->pose.position.x;
|
||||
cur_pos.y = msg->pose.position.y;
|
||||
// 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
|
||||
/-------------------------------------------------------------*/
|
||||
{
|
||||
// DEBUG
|
||||
// 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
|
||||
/-------------------------------------------------------------*/
|
||||
|
|
@ -925,9 +935,9 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw)
|
|||
moveMsg.header.frame_id = 1;
|
||||
|
||||
// DEBUG
|
||||
// ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f", local_pos_new[0], local_pos_new[1], x, y);
|
||||
moveMsg.pose.position.x = local_pos_new[0] + y;
|
||||
moveMsg.pose.position.y = local_pos_new[1] + x;
|
||||
// ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f", cur_pos.x, cur_pos.y, x, y);
|
||||
moveMsg.pose.position.x = cur_pos.x + y;
|
||||
moveMsg.pose.position.y = cur_pos.y + x;
|
||||
moveMsg.pose.position.z = z;
|
||||
|
||||
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));
|
||||
buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0], neighbours_pos_payload[1],
|
||||
neighbours_pos_payload[2]);
|
||||
GPS nei_pos;
|
||||
POSE nei_pos;
|
||||
nei_pos.latitude = neighbours_pos_payload[0];
|
||||
nei_pos.longitude = neighbours_pos_payload[1];
|
||||
nei_pos.altitude = neighbours_pos_payload[2];
|
||||
|
|
|
|||
Loading…
Reference in New Issue