Neighbours pos publisher addition
This commit is contained in:
parent
4dea03e334
commit
177a0cd20d
|
@ -11,6 +11,21 @@ find_package(catkin REQUIRED COMPONENTS
|
||||||
std_msgs
|
std_msgs
|
||||||
mavros_msgs
|
mavros_msgs
|
||||||
sensor_msgs
|
sensor_msgs
|
||||||
|
message_generation
|
||||||
|
)
|
||||||
|
|
||||||
|
##############################
|
||||||
|
#Generate messages#
|
||||||
|
##############################
|
||||||
|
|
||||||
|
add_message_files(
|
||||||
|
FILES
|
||||||
|
neigh_pos.msg
|
||||||
|
)
|
||||||
|
|
||||||
|
generate_messages(
|
||||||
|
DEPENDENCIES
|
||||||
|
sensor_msgs
|
||||||
)
|
)
|
||||||
|
|
||||||
###################################
|
###################################
|
||||||
|
@ -20,7 +35,7 @@ find_package(catkin REQUIRED COMPONENTS
|
||||||
catkin_package(
|
catkin_package(
|
||||||
INCLUDE_DIRS include
|
INCLUDE_DIRS include
|
||||||
# LIBRARIES xbee_ros_node
|
# LIBRARIES xbee_ros_node
|
||||||
CATKIN_DEPENDS roscpp std_msgs mavros_msgs sensor_msgs
|
CATKIN_DEPENDS roscpp std_msgs mavros_msgs sensor_msgs message_runtime
|
||||||
# DEPENDS system_lib
|
# DEPENDS system_lib
|
||||||
)
|
)
|
||||||
|
|
||||||
|
@ -41,6 +56,7 @@ add_executable(rosbuzz_node src/rosbuzz.cpp
|
||||||
src/uav_utility.cpp
|
src/uav_utility.cpp
|
||||||
src/buzz_update.cpp)
|
src/buzz_update.cpp)
|
||||||
target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} /usr/local/lib/libbuzz.so /usr/local/lib/libbuzzdbg.so -lpthread)
|
target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} /usr/local/lib/libbuzz.so /usr/local/lib/libbuzzdbg.so -lpthread)
|
||||||
|
add_dependencies(rosbuzz_node rosbuzz_generate_messages_cpp)
|
||||||
|
|
||||||
# Executables and libraries for installation to do
|
# Executables and libraries for installation to do
|
||||||
install(TARGETS rosbuzz_node
|
install(TARGETS rosbuzz_node
|
||||||
|
|
|
@ -9,6 +9,7 @@
|
||||||
#include "mavros_msgs/BatteryStatus.h"
|
#include "mavros_msgs/BatteryStatus.h"
|
||||||
#include "mavros_msgs/Mavlink.h"
|
#include "mavros_msgs/Mavlink.h"
|
||||||
#include "sensor_msgs/NavSatStatus.h"
|
#include "sensor_msgs/NavSatStatus.h"
|
||||||
|
#include <rosbuzz/neigh_pos.h>
|
||||||
#include <sstream>
|
#include <sstream>
|
||||||
#include <buzz/buzzasm.h>
|
#include <buzz/buzzasm.h>
|
||||||
#include "buzz_utility.h"
|
#include "buzz_utility.h"
|
||||||
|
@ -38,6 +39,7 @@ private:
|
||||||
double cur_pos[3];
|
double cur_pos[3];
|
||||||
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;
|
||||||
|
|
||||||
int timer_step=0;
|
int timer_step=0;
|
||||||
int robot_id=0;
|
int robot_id=0;
|
||||||
//int oldcmdID=0;
|
//int oldcmdID=0;
|
||||||
|
@ -47,6 +49,7 @@ private:
|
||||||
bool rcclient;
|
bool rcclient;
|
||||||
ros::ServiceClient mav_client;
|
ros::ServiceClient mav_client;
|
||||||
ros::Publisher payload_pub;
|
ros::Publisher payload_pub;
|
||||||
|
ros::Publisher neigh_pos_pub;
|
||||||
ros::ServiceServer service;
|
ros::ServiceServer service;
|
||||||
ros::Subscriber current_position_sub;
|
ros::Subscriber current_position_sub;
|
||||||
ros::Subscriber battery_sub;
|
ros::Subscriber battery_sub;
|
||||||
|
|
|
@ -0,0 +1,2 @@
|
||||||
|
Header header
|
||||||
|
sensor_msgs/NavSatFix[] pos_neigh
|
|
@ -48,8 +48,8 @@
|
||||||
<run_depend>mavros_msgs</run_depend>
|
<run_depend>mavros_msgs</run_depend>
|
||||||
<build_depend>sensor_msgs</build_depend>
|
<build_depend>sensor_msgs</build_depend>
|
||||||
<run_depend>sensor_msgs</run_depend>
|
<run_depend>sensor_msgs</run_depend>
|
||||||
<!-- <build_depend>message_generation</build_depend> -->
|
<build_depend>message_generation</build_depend>
|
||||||
<!-- <run_depend>message_runtime</run_depend> -->
|
<run_depend>message_runtime</run_depend>
|
||||||
|
|
||||||
<!-- The export tag contains other, unspecified, tags -->
|
<!-- The export tag contains other, unspecified, tags -->
|
||||||
<export>
|
<export>
|
||||||
|
|
|
@ -18,7 +18,7 @@ namespace buzz_utility{
|
||||||
static uint8_t* BO_BUF = 0;
|
static uint8_t* BO_BUF = 0;
|
||||||
static buzzdebug_t DBG_INFO = 0;
|
static buzzdebug_t DBG_INFO = 0;
|
||||||
static uint8_t MSG_SIZE = 100; // Only 100 bytes of Buzz messages every step
|
static uint8_t MSG_SIZE = 100; // Only 100 bytes of Buzz messages every step
|
||||||
static int MAX_MSG_SIZE = 10100; // Maximum Msg size for sending update packets
|
static int MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
|
||||||
|
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
|
@ -37,6 +37,19 @@ namespace rosbzz_node{
|
||||||
{
|
{
|
||||||
/*Update neighbors position inside Buzz*/
|
/*Update neighbors position inside Buzz*/
|
||||||
buzz_utility::neighbour_pos_callback(neighbours_pos_map);
|
buzz_utility::neighbour_pos_callback(neighbours_pos_map);
|
||||||
|
map< int, buzz_utility::Pos_struct >::iterator it;
|
||||||
|
rosbuzz::neigh_pos neigh_pos_array; //neigh_pos_array.clear();
|
||||||
|
sensor_msgs::NavSatFix tmp;
|
||||||
|
for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){
|
||||||
|
tmp.position_covariance_type=it->first; //custom robot id storage
|
||||||
|
tmp.longitude=(it->second).x;
|
||||||
|
tmp.latitude=(it->second).y;
|
||||||
|
tmp.altitude=(it->second).z;
|
||||||
|
|
||||||
|
neigh_pos_array.pos_neigh.push_back(tmp);
|
||||||
|
|
||||||
|
}
|
||||||
|
neigh_pos_pub.publish(neigh_pos_array);
|
||||||
/*Check updater state and step code*/
|
/*Check updater state and step code*/
|
||||||
if(update_routine(bcfname.c_str(), dbgfname.c_str(),0)==CODE_RUNNING)
|
if(update_routine(bcfname.c_str(), dbgfname.c_str(),0)==CODE_RUNNING)
|
||||||
/*Step buzz script */
|
/*Step buzz script */
|
||||||
|
@ -99,7 +112,7 @@ namespace rosbzz_node{
|
||||||
flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_status_update,this);
|
flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_status_update,this);
|
||||||
/*publishers*/
|
/*publishers*/
|
||||||
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
|
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
|
||||||
cout<<out_payload<<endl;
|
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
|
||||||
/* Clients*/
|
/* Clients*/
|
||||||
mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
|
mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
|
||||||
|
|
||||||
|
@ -286,7 +299,7 @@ namespace rosbzz_node{
|
||||||
buzz_utility::in_msg_process((message_obt+3));
|
buzz_utility::in_msg_process((message_obt+3));
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* RC command service */
|
/* RC command service */
|
||||||
bool roscontroller::rc_callback(mavros_msgs::CommandInt::Request &req,
|
bool roscontroller::rc_callback(mavros_msgs::CommandInt::Request &req,
|
||||||
mavros_msgs::CommandInt::Response &res){
|
mavros_msgs::CommandInt::Response &res){
|
||||||
|
|
|
@ -19,32 +19,20 @@ function barrier_set(threshold, transf) {
|
||||||
barrier_wait(threshold, transf);
|
barrier_wait(threshold, transf);
|
||||||
}
|
}
|
||||||
barrier = stigmergy.create(BARRIER_VSTIG)
|
barrier = stigmergy.create(BARRIER_VSTIG)
|
||||||
statestr = "barrier"
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#
|
#
|
||||||
# Make yourself ready
|
# Make yourself ready
|
||||||
#
|
#
|
||||||
function barrier_ready() {
|
function barrier_ready() {
|
||||||
|
|
||||||
barrier.put(id, 1)
|
barrier.put(id, 1)
|
||||||
print ("before put")
|
|
||||||
barrier.get(id)
|
|
||||||
print("barrier put : ")
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#
|
#
|
||||||
# Executes the barrier
|
# Executes the barrier
|
||||||
#
|
#
|
||||||
function table_print(t) {
|
|
||||||
foreach(t, function(key, value) {
|
|
||||||
log(key, " -> ", value)
|
|
||||||
})
|
|
||||||
}
|
|
||||||
function barrier_wait(threshold, transf) {
|
function barrier_wait(threshold, transf) {
|
||||||
#table_print(barrier)
|
|
||||||
barrier.get(id)
|
barrier.get(id)
|
||||||
extradbg = barrier.size()
|
|
||||||
if(barrier.size() >= threshold) {
|
if(barrier.size() >= threshold) {
|
||||||
barrier = nil
|
barrier = nil
|
||||||
transf()
|
transf()
|
||||||
|
@ -69,7 +57,7 @@ neighbors.listen("cmd",
|
||||||
|
|
||||||
function takeoff() {
|
function takeoff() {
|
||||||
if( flight.status == 2) {
|
if( flight.status == 2) {
|
||||||
barrier_set(ROBOTS,transition_to_land)
|
barrier_set(ROBOTS,idle)
|
||||||
barrier_ready()
|
barrier_ready()
|
||||||
}
|
}
|
||||||
else if(flight.status!=3)
|
else if(flight.status!=3)
|
||||||
|
@ -84,31 +72,14 @@ function land() {
|
||||||
uav_land()
|
uav_land()
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
function transition_to_land() {
|
|
||||||
statef=transition_to_land
|
|
||||||
if(battery.capacity<50){
|
|
||||||
print("Low battery! Landing the fleet")
|
|
||||||
statef=land
|
|
||||||
neighbors.broadcast("cmd", 21)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
# Executed once at init time.
|
# Executed once at init time.
|
||||||
function init() {
|
function init() {
|
||||||
i = 0
|
statef=idle
|
||||||
a = 0
|
|
||||||
val = 0
|
|
||||||
oldcmd = 0
|
|
||||||
tim=0
|
|
||||||
statef=idle
|
|
||||||
}
|
}
|
||||||
|
|
||||||
# Executed at each time step.
|
# Executed at each time step.
|
||||||
function step() {
|
function step() {
|
||||||
|
|
||||||
#if(flight.rc_cmd!=oldcmd) {
|
|
||||||
|
|
||||||
if(flight.rc_cmd==22 and flight.status==1) {
|
if(flight.rc_cmd==22 and flight.status==1) {
|
||||||
flight.rc_cmd=0
|
flight.rc_cmd=0
|
||||||
statef=takeoff
|
statef=takeoff
|
||||||
|
@ -118,8 +89,6 @@ function step() {
|
||||||
statef=land
|
statef=land
|
||||||
neighbors.broadcast("cmd", 21)
|
neighbors.broadcast("cmd", 21)
|
||||||
}
|
}
|
||||||
# oldcmd=flight.rc_cmd
|
|
||||||
#}
|
|
||||||
|
|
||||||
statef()
|
statef()
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue