From 177a0cd20dec7cb8567e90a28e66f6df90a84fb9 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Mon, 9 Jan 2017 17:15:33 -0500 Subject: [PATCH] Neighbours pos publisher addition --- CMakeLists.txt | 18 +++++++++++++++++- include/roscontroller.h | 3 +++ msg/neigh_pos.msg | 2 ++ package.xml | 4 ++-- src/buzz_utility.cpp | 2 +- src/roscontroller.cpp | 17 +++++++++++++++-- src/test1.bzz | 37 +++---------------------------------- 7 files changed, 43 insertions(+), 40 deletions(-) create mode 100644 msg/neigh_pos.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 98e62f3..0df4d4c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,6 +11,21 @@ find_package(catkin REQUIRED COMPONENTS std_msgs mavros_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( INCLUDE_DIRS include # 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 ) @@ -41,6 +56,7 @@ add_executable(rosbuzz_node src/rosbuzz.cpp src/uav_utility.cpp src/buzz_update.cpp) 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 install(TARGETS rosbuzz_node diff --git a/include/roscontroller.h b/include/roscontroller.h index c03298b..e993136 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -9,6 +9,7 @@ #include "mavros_msgs/BatteryStatus.h" #include "mavros_msgs/Mavlink.h" #include "sensor_msgs/NavSatStatus.h" +#include #include #include #include "buzz_utility.h" @@ -38,6 +39,7 @@ private: double cur_pos[3]; uint64_t payload; std::map< int, buzz_utility::Pos_struct> neighbours_pos_map; + int timer_step=0; int robot_id=0; //int oldcmdID=0; @@ -47,6 +49,7 @@ private: bool rcclient; ros::ServiceClient mav_client; ros::Publisher payload_pub; + ros::Publisher neigh_pos_pub; ros::ServiceServer service; ros::Subscriber current_position_sub; ros::Subscriber battery_sub; diff --git a/msg/neigh_pos.msg b/msg/neigh_pos.msg new file mode 100644 index 0000000..8db4257 --- /dev/null +++ b/msg/neigh_pos.msg @@ -0,0 +1,2 @@ +Header header +sensor_msgs/NavSatFix[] pos_neigh diff --git a/package.xml b/package.xml index cd839c4..6f4fc98 100644 --- a/package.xml +++ b/package.xml @@ -48,8 +48,8 @@ mavros_msgs sensor_msgs sensor_msgs - - + message_generation + message_runtime diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 2de54e7..df62d1b 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -18,7 +18,7 @@ namespace buzz_utility{ static uint8_t* BO_BUF = 0; static buzzdebug_t DBG_INFO = 0; 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 /****************************************/ diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 0f07329..4b685b9 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -37,6 +37,19 @@ namespace rosbzz_node{ { /*Update neighbors position inside Buzz*/ 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*/ if(update_routine(bcfname.c_str(), dbgfname.c_str(),0)==CODE_RUNNING) /*Step buzz script */ @@ -99,7 +112,7 @@ namespace rosbzz_node{ flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_status_update,this); /*publishers*/ payload_pub = n_c.advertise(out_payload, 1000); - cout<("/neighbours_pos",1000); /* Clients*/ mav_client = n_c.serviceClient(fcclient_name); @@ -286,7 +299,7 @@ namespace rosbzz_node{ buzz_utility::in_msg_process((message_obt+3)); } - + /* RC command service */ bool roscontroller::rc_callback(mavros_msgs::CommandInt::Request &req, mavros_msgs::CommandInt::Response &res){ diff --git a/src/test1.bzz b/src/test1.bzz index 490e954..6c9a0dc 100644 --- a/src/test1.bzz +++ b/src/test1.bzz @@ -19,32 +19,20 @@ function barrier_set(threshold, transf) { barrier_wait(threshold, transf); } barrier = stigmergy.create(BARRIER_VSTIG) - statestr = "barrier" } # # Make yourself ready # function barrier_ready() { - barrier.put(id, 1) - print ("before put") - barrier.get(id) - print("barrier put : ") } # # Executes the barrier # -function table_print(t) { - foreach(t, function(key, value) { - log(key, " -> ", value) - }) -} function barrier_wait(threshold, transf) { - #table_print(barrier) barrier.get(id) - extradbg = barrier.size() if(barrier.size() >= threshold) { barrier = nil transf() @@ -69,7 +57,7 @@ neighbors.listen("cmd", function takeoff() { if( flight.status == 2) { - barrier_set(ROBOTS,transition_to_land) + barrier_set(ROBOTS,idle) barrier_ready() } else if(flight.status!=3) @@ -84,31 +72,14 @@ function 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. function init() { -i = 0 -a = 0 -val = 0 -oldcmd = 0 -tim=0 -statef=idle + statef=idle } - + # Executed at each time step. function step() { -#if(flight.rc_cmd!=oldcmd) { - if(flight.rc_cmd==22 and flight.status==1) { flight.rc_cmd=0 statef=takeoff @@ -118,8 +89,6 @@ function step() { statef=land neighbors.broadcast("cmd", 21) } -# oldcmd=flight.rc_cmd -#} statef() }