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

View File

@ -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

View File

@ -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
}

View File

@ -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.

View File

@ -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
*/

View File

@ -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();
};
}

View File

@ -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;
}

View File

@ -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];