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

add gps goal control, re-structured launch dir and add height to buzz moveto closure

This commit is contained in:
dave 2017-08-25 17:05:40 -04:00
parent 5804aace94
commit 13267a9e97
16 changed files with 261 additions and 452 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
buzz_scripts/flock.bzz
View File

@ -38,7 +38,7 @@ function action() {
# Move according to vector # Move according to vector
print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum)) print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum))
uav_moveto(accum.x, accum.y) uav_moveto(accum.x, accum.y, 0.0)
UAVSTATE = "LENNARDJONES" UAVSTATE = "LENNARDJONES"
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS # if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
@ -47,11 +47,11 @@ function action() {
# } else if(timeW>=WAIT_TIMEOUT/2) { # } else if(timeW>=WAIT_TIMEOUT/2) {
# UAVSTATE ="GOEAST" # UAVSTATE ="GOEAST"
# timeW = timeW+1 # timeW = timeW+1
# uav_moveto(0.0,5.0) # uav_moveto(0.0, 5.0, 0.0)
# } else { # } else {
# UAVSTATE ="GONORTH" # UAVSTATE ="GONORTH"
# timeW = timeW+1 # timeW = timeW+1
# uav_moveto(5.0,0.0) # uav_moveto(5.0, 0.0, 0.0)
# } # }
} }

20
buzz_scripts/graphform.bzz
View File

@ -446,7 +446,7 @@ function TransitionToJoined(){
m_navigation.x=0.0 m_navigation.x=0.0
m_navigation.y=0.0 m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
} }
# #
@ -471,7 +471,7 @@ while(i<m_neighbourCount){
} }
m_navigation.x=0.0 m_navigation.x=0.0
m_navigation.y=0.0 m_navigation.y=0.0
uav_moveto(m_navigation.x,m_navigation.y) uav_moveto(m_navigation.x, m_navigation.y, 0.0)
} }
# #
@ -535,7 +535,7 @@ function DoFree() {
# Limit the mvt # Limit the mvt
if(math.vec2.length(m_navigation)>ROBOT_MAXVEL*2) if(math.vec2.length(m_navigation)>ROBOT_MAXVEL*2)
m_navigation = math.vec2.scale(m_navigation, ROBOT_MAXVEL*2/math.vec2.length(m_navigation)) m_navigation = math.vec2.scale(m_navigation, ROBOT_MAXVEL*2/math.vec2.length(m_navigation))
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
}else{ #no joined robots in sight }else{ #no joined robots in sight
i=0 i=0
var tempvec={.x=0.0,.y=0.0} var tempvec={.x=0.0,.y=0.0}
@ -545,7 +545,7 @@ function DoFree() {
i=i+1 i=i+1
} }
m_navigation=math.vec2.scale(tempvec,1.0/i) m_navigation=math.vec2.scale(tempvec,1.0/i)
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
} }
@ -554,7 +554,7 @@ function DoFree() {
if(step_cunt<=1){ if(step_cunt<=1){
m_navigation.x=0.0 m_navigation.x=0.0
m_navigation.y=0.0 m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
} }
#set message #set message
m_selfMessage.State=s2i(m_eState) m_selfMessage.State=s2i(m_eState)
@ -614,7 +614,7 @@ function DoAsking(){
} }
m_navigation.x=0.0 m_navigation.x=0.0
m_navigation.y=0.0 m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
} }
# #
@ -662,7 +662,7 @@ function DoJoining(){
S2Target = math.vec2.scale(S2Target, ROBOT_MAXVEL/math.vec2.length(S2Target)) S2Target = math.vec2.scale(S2Target, ROBOT_MAXVEL/math.vec2.length(S2Target))
m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing) m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing)
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
@ -760,7 +760,7 @@ function DoJoined(){
m_navigation.x=0.0 m_navigation.x=0.0
m_navigation.y=0.0 m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
#check if should to transists to lock #check if should to transists to lock
@ -789,7 +789,7 @@ if(m_nLabel!=0){
m_navigation=motion_vector() m_navigation=motion_vector()
} }
#move #move
uav_moveto(m_navigation.x,m_navigation.y) uav_moveto(m_navigation.x, m_navigation.y, 0.0)
} }
function action(){ function action(){
@ -912,7 +912,7 @@ function destroy() {
#clear neighbour message #clear neighbour message
m_navigation.x=0.0 m_navigation.x=0.0
m_navigation.y=0.0 m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
m_vecNodes={} m_vecNodes={}
#stop listening #stop listening
neighbors.ignore("m") neighbors.ignore("m")

73
buzz_scripts/include/uavstates.bzz
View File

@ -5,8 +5,10 @@
######################################## ########################################
TARGET_ALTITUDE = 5.0 TARGET_ALTITUDE = 5.0
UAVSTATE = "TURNEDOFF" UAVSTATE = "TURNEDOFF"
GOTO_MAXVEL = 2
goal = {.range=0, .bearing=0}
function uav_initswarm(){ function uav_initswarm() {
s = swarm.create(1) s = swarm.create(1)
s.join() s.join()
statef=turnedoff statef=turnedoff
@ -26,13 +28,10 @@ function idle() {
function takeoff() { function takeoff() {
UAVSTATE = "TAKEOFF" UAVSTATE = "TAKEOFF"
statef=takeoff statef=takeoff
#log("TakeOff: ", flight.status)
#log("Relative position: ", position.altitude)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,action,land) barrier_set(ROBOTS,action,land)
barrier_ready() barrier_ready()
#statef=hexagon
} }
else { else {
log("Altitude: ", TARGET_ALTITUDE) log("Altitude: ", TARGET_ALTITUDE)
@ -44,7 +43,7 @@ function takeoff() {
function land() { function land() {
UAVSTATE = "LAND" UAVSTATE = "LAND"
statef=land statef=land
#log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){ if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21) neighbors.broadcast("cmd", 21)
uav_land() uav_land()
@ -58,6 +57,28 @@ function land() {
} }
} }
function goto() {
UAVSTATE = "GOTO"
statef=goto
# print(rc_goto.id, " is going alone to lat=", rc_goto.latitude)
if(rc_goto.id==id){
s.leave()
rb_from_gps(rc_goto.latitude, rc_goto.longitude)
print(id, " has to move ", goal.range)
m_navigation = math.vec2.newp(goal.range, goal.bearing)
if(math.vec2.length(m_navigation)>GOTO_MAXVEL) {
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)
} else if(math.vec2.length(m_navigation)>0.25)
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
else
statef = idle
} else {
neighbors.broadcast("cmd", 16)
neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude)
}
}
function follow() { function follow() {
if(size(targets)>0) { if(size(targets)>0) {
UAVSTATE = "FOLLOW" UAVSTATE = "FOLLOW"
@ -67,7 +88,7 @@ function follow() {
force=(0.05)*(tab.range)^4 force=(0.05)*(tab.range)^4
attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing)) attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing))
}) })
uav_moveto(attractor.x, attractor.y) uav_moveto(attractor.x, attractor.y, 0.0)
} else { } else {
log("No target in local table!") log("No target in local table!")
#statef=idle #statef=idle
@ -90,11 +111,8 @@ function uav_rccmd() {
neighbors.broadcast("cmd", 21) neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) { } else if(flight.rc_cmd==16) {
flight.rc_cmd=0 flight.rc_cmd=0
UAVSTATE = "FOLLOW" UAVSTATE = "GOTO"
log(rc_goto.latitude, " ", rc_goto.longitude) statef = goto
add_targetrb(0,rc_goto.latitude,rc_goto.longitude)
statef = follow
#uav_goto()
} else if(flight.rc_cmd==400) { } else if(flight.rc_cmd==400) {
flight.rc_cmd=0 flight.rc_cmd=0
uav_arm() uav_arm()
@ -114,13 +132,38 @@ function uav_neicmd() {
function(vid, value, rid) { function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid) print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22 and UAVSTATE!="TAKEOFF") { if(value==22 and UAVSTATE!="TAKEOFF") {
statef=takeoff statef=takeoff
} else if(value==21) { } else if(value==21) {
statef=land statef=land
} else if(value==400 and UAVSTATE=="IDLE") { } else if(value==400 and UAVSTATE=="IDLE") {
uav_arm() uav_arm()
} else if(value==401 and UAVSTATE=="IDLE"){ } else if(value==401 and UAVSTATE=="IDLE"){
uav_disarm() uav_disarm()
} else if(value==16){
neighbors.listen("gt",function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
# uav_storegoal(lat, lon, alt)
})
statef=goto
} }
}) })
} }
function LimitAngle(angle){
if(angle>2*math.pi)
return angle-2*math.pi
else if (angle<0)
return angle+2*math.pi
else
return angle
}
function rb_from_gps(lat, lon) {
print("rb dbg: ",lat,lon,position.latitude,position.longitude)
d_lon = lon - position.longitude
d_lat = lat - position.latitude
ned_x = d_lat/180*math.pi * 6371000.0;
ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi);
goal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
goal.bearing = LimitAngle(math.atan(ned_y,ned_x));
}

9
buzz_scripts/testalone.bzz
View File

@ -7,17 +7,22 @@ include "vstigenv.bzz"
function action() { function action() {
statef=action statef=action
# test moveto cmd dx, dy # test moveto cmd dx, dy
# uav_moveto(0.5, 0.5) # uav_moveto(0.5, 0.5, 0.0)
} }
# Executed once at init time. # Executed once at init time.
function init() { function init() {
uav_initswarm()
uav_initstig()
} }
# Executed at each time step. # Executed at each time step.
function step() { function step() {
log("Altitude: ", position.altitude)
uav_rccmd() uav_rccmd()
statef()
log("Current state: ", UAVSTATE)
} }
# 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

@ -36,7 +36,7 @@ int buzzros_print(buzzvm_t vm);
* commands the UAV to go to a position supplied * commands the UAV to go to a position supplied
*/ */
int buzzuav_moveto(buzzvm_t vm); int buzzuav_moveto(buzzvm_t vm);
int buzzuav_goto(buzzvm_t vm); int buzzuav_storegoal(buzzvm_t vm);
/* Returns the current command from local variable*/ /* Returns the current command from local variable*/
int getcmd(); int getcmd();
/*Sets goto position */ /*Sets goto position */

59
launch/husky.launch
View File

@ -1,59 +0,0 @@
<launch>
<group ns="gps">
<!-- NavSat Serial -->
<node pkg="nmea_comms" type="serial_node" name="nmea_serial_node" output="screen">
<param name="port" value="$(optenv HUSKY_NAVSAT_PORT /dev/clearpath/gps)" />
<param name="baud" value="$(optenv HUSKY_NAVSAT_BAUD 19200)" />
</node>
<!-- NavSat Processing -->
<node pkg="nmea_navsat_driver" type="nmea_topic_driver" name="nmea_topic_driver">
</node>
</group>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" >
<rosparam>
magnetic_declination_radians: 0
roll_offset: 0
pitch_offset: 0
yaw_offset: 0
zero_altitude: false
broadcast_utm_transform: false
</rosparam>
</node>
<!-- run xbee>
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" / -->
<!-- xmee_mav Drone type and Commununication Mode -->
<!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="master solo" output="screen" -->
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
<!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave solo" output="screen" -->
<!--node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" -->
<!-- xmee_mav Topics and Services Names -->
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
<node pkg="dji_sdk_mistlab" type="dji_sdk_mav" name="dji_sdk_mav" output="screen"/>
<!-- run rosbuzz -->
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="$(find rosbuzz)/launch/launch_config/husky.yaml"/>
<param name="bzzfile_name" value="$(find rosbuzz)/script/testflockfev.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="buzzcmd" />
<param name="in_payload" value="inMavlink"/>
<param name="out_payload" value="outMavlink"/>
<param name="xbee_status_srv" value="xbee_status"/>
<param name="xbee_plugged" value="true"/>
<param name="name" value="husky1"/>
<param name="stand_by" value="$(find rosbuzz)/script/stand_by.bzz"/>
</node>
</launch>

12
launch/launch_config/topics.yaml Normal file
View File

@ -0,0 +1,12 @@
topics:
gps : global_position/global
battery : battery
status : state
estatus : extended_state
fcclient: cmd/command
setpoint: setpoint_position/local
armclient: cmd/arming
modeclient: set_mode
localpos: local_position/pose
stream: set_stream_rate
altitude: global_position/rel_alt

57
launch/rosbuzz-solo.launch
View File

@ -1,57 +0,0 @@
<launch>
<!-- RUN mavros -->
<arg name="fcu_url" default="/dev/ttyS0:115200" />
<arg name="gcs_url" default="" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />
<arg name="log_output" default="screen" />
<include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" />
<arg name="tgt_component" value="$(arg tgt_component)" />
<arg name="log_output" value="$(arg log_output)" />
</include>
<!-- set streaming rate -->
<!-- node pkg="rosservice" type="rosservice" name="freq" args="call /mavros/set_stream_rate 0 10 1" output="screen" / -->
<!-- run xbee>
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" / -->
<!-- xmee_mav Drone type and Commununication Mode -->
<!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="master solo" output="screen" -->
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
<!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave solo" output="screen" -->
<!--node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" -->
<!-- xmee_mav Topics and Services Names -->
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
<param name="USB_port" type="str" value="/dev/ttyUSB0" />
<param name="Baud_rate" type="double" value="230400" />
<!-- run rosbuzz -->
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="$(find rosbuzz)/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/flock.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="buzzcmd" />
<param name="in_payload" value="inMavlink"/>
<param name="out_payload" value="outMavlink"/>
<param name="xbee_status_srv" value="xbee_status"/>
<param name="xbee_plugged" value="true"/>
<param name="name" value="solos1"/>
<param name="stand_by" value="$(find rosbuzz)/buzz_scripts/stand_by.bzz"/>
</node>
</launch>

51
launch/rosbuzz-testing-sitl.launch
View File

@ -1,51 +0,0 @@
<launch>
<!-- RUN mavros
<arg name="fcu_url" default="tcp://127.0.0.1:5760" />
<arg name="gcs_url" default="" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />
<arg name="log_output" default="screen" />
<include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" />
<arg name="tgt_component" value="$(arg tgt_component)" />
<arg name="log_output" value="$(arg log_output)" />
</include>
-->
<!-- run xbee -->
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
<!-- run rosbuzz -->
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="buzzcmd" />
<param name="in_payload" value="inMavlink"/>
<param name="out_payload" value="outMavlink"/>
<param name="xbee_status_srv" value="xbee_status"/>
<param name="xbee_plugged" value="false"/>
<param name="name" value="solos1"/>
<param name="stand_by" value="/home/ivan/catkin_ws/src/rosbuzz/script/stand_by.bzz"/>
</node>
<!--
<node pkg="rosloader" type="rosloader" name="rosloader" output="screen"/>
-->
<!-- set streaming rate
/mavros/cmd/arming
<node pkg="rosservice" type="rosservice" name="stream_rate" args="call /mavros/set_stream_rate 0 10 1" />
-->
</launch>

43
launch/rosbuzz-testing.launch
View File

@ -1,43 +0,0 @@
<launch>
<!-- RUN mavros -->
<arg name="fcu_url" default="/dev/ttyAMA0:115200" />
<arg name="gcs_url" default="" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />
<arg name="log_output" default="screen" />
<include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" />
<arg name="tgt_component" value="$(arg tgt_component)" />
<arg name="log_output" value="$(arg log_output)" />
</include>
<!-- set streaming rate -->
<!-- node pkg="rosservice" type="rosservice" name="freq" args="call /mavros/set_stream_rate 0 10 1" output="screen" / -->
<!-- run xbee -->
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
<param name="No_of_dev" type="int" value="3" />
<!-- run rosbuzz -->
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="/home/pi/ros_catkin_ws/src/ROSBuzz/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="/home/pi/ros_catkin_ws/src/ROSBuzz/src/testflockfev.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="/buzzcmd" />
<param name="in_payload" value="/inMavlink"/>
<param name="out_payload" value="/outMavlink"/>
<param name="robot_id" value="3"/>
<param name="No_of_Robots" value="3"/>
<param name="stand_by" value="/home/pi/ros_catkin_ws/src/ROSBuzz/src/stand_by.bo"/>
</node>
</launch>

32
launch/rosbuzz.launch
View File

@ -1,14 +1,22 @@
<!-- Launch file for ROSBuzz --> <?xml version="1.0"?>
<!-- Generic launch file for ROSBuzz -->
<!-- This file is included in all ROS workspace launch files -->
<!-- Modify with great care! -->
<launch> <launch>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" > <arg name="name" default="robot0"/>
<rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/> <arg name="xbee_plugged" default="true"/>
<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/src/testmoveto.bzz" /> <arg name="script" default="testalone"/>
<param name="rcclient" value="true" />
<param name="rcservice_name" value="/buzzcmd" /> <node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" launch-prefix="gdb -ex run --args">
<param name="in_payload" value="/inMavlink"/> <rosparam file="$(find rosbuzz)/launch/launch_config/topics.yaml"/>
<param name="out_payload" value="/outMavlink"/> <param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
<param name="xbee_status_srv" value="/xbee_status"/> <param name="rcclient" value="true" />
<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/> <param name="rcservice_name" value="buzzcmd" />
</node> <param name="in_payload" value="inMavlink"/>
<param name="out_payload" value="outMavlink"/>
<param name="xbee_plugged" value="$(arg xbee_plugged)"/>
<param name="name" value="$(arg name)"/>
<param name="xbee_status_srv" value="xbee_status"/>
<param name="stand_by" value="$(find rosbuzz)/buzz_scripts/stand_by.bzz"/>
</node>
</launch> </launch>

27
launch/rosbuzz_2_parallel.launch
View File

@ -1,27 +0,0 @@
<!-- Launch file for ROSBuzz -->
<launch>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/testflockfev_2parallel.bzz" />
<rosparam file="/home/vivek/catkin_ws/src/rosbuzz/launch/launch_config/m100.yaml"/>
<param name="rcclient" value="true" />
<param name="rcservice_name" value="rc_cmd" />
<param name="fcclient_name" value="/djicmd" />
<param name="in_payload" value="/inMavlink"/>
<!-- param name="in_payload" value="/rosbuzz_node1/outMavlink"-->
<param name="out_payload" value="/outMavlink"/>
<param name="robot_id" value="1"/>
<param name="No_of_Robots" value="1"/>
<param name="stand_by" value="/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bo"/>
</node>
<!-- node name="rosbuzz_node1" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" -->
<!-- param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/test1.bzz" -->
<!-- param name="rcclient" value="true" -->
<!-- param name="rcservice_name" value="rc_cmd" -->
<!-- param name="fcclient_name" value="/djicmd" -->
<!-- param name="in_payload" value="/rosbuzz_node/outMavlink"-->
<!-- param name="out_payload" value="outMavlink"-->
<!-- param name="robot_id" value="2"-->
<!-- node-->
</launch>

16
launch/rosbuzzm100.launch
View File

@ -1,16 +0,0 @@
<!-- Launch file for ROSBuzz -->
<launch>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen">
<rosparam file="$(find rosbuzz)/launch/launch_config/m100.yaml"/>
<param name="bzzfile_name" value="$(find rosbuzz)/script/testflockfev.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="buzzcmd" />
<param name="in_payload" value="inMavlink"/>
<param name="out_payload" value="outMavlink"/>
<param name="xbee_plugged" value="true"/>
<param name="name" value="m1001"/>
<param name="xbee_status_srv" value="xbee_status"/>
<param name="stand_by" value="$(find rosbuzz)/script/stand_by.bzz"/>
</node>
</launch>

6
src/buzz_utility.cpp
View File

@ -304,8 +304,8 @@ void in_message_process(){
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm));
@ -349,7 +349,7 @@ void in_message_process(){
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));

95
src/buzzuav_closures.cpp
View File

@ -123,27 +123,27 @@ namespace buzzuav_closures{
/ Buzz closure to move following a 2D vector / Buzz closure to move following a 2D vector
/----------------------------------------*/ /----------------------------------------*/
int buzzuav_moveto(buzzvm_t vm) { int buzzuav_moveto(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 2); buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); /* dx */ buzzvm_lload(vm, 1); /* dx */
buzzvm_lload(vm, 2); /* dy */ buzzvm_lload(vm, 2); /* dy */
//buzzvm_lload(vm, 3); /* Latitude */ buzzvm_lload(vm, 3); /* dheight */
//buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
float dy = buzzvm_stack_at(vm, 1)->f.value; float dh = buzzvm_stack_at(vm, 1)->f.value;
float dx = buzzvm_stack_at(vm, 2)->f.value; float dy = buzzvm_stack_at(vm, 2)->f.value;
double d = sqrt(dx*dx+dy*dy); //range float dx = buzzvm_stack_at(vm, 3)->f.value;
goto_pos[0]=dx; goto_pos[0]=dx;
goto_pos[1]=dy; goto_pos[1]=dy;
goto_pos[2]=height; goto_pos[2]=height+dh;
/*double b = atan2(dy,dx); //bearing /*double b = atan2(dy,dx); //bearing
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
gps_from_rb(d, b, goto_pos); gps_from_rb(d, b, goto_pos);
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/ cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
//printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); //printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
//ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], goto_pos[1], goto_pos[2]); //ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], goto_pos[1], goto_pos[2]);
buzz_cmd= COMMAND_MOVETO; // TO DO what should we use buzz_cmd= COMMAND_MOVETO; // TO DO what should we use
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
int buzzuav_update_targets(buzzvm_t vm) { int buzzuav_update_targets(buzzvm_t vm) {
@ -224,23 +224,23 @@ namespace buzzuav_closures{
int buzzuav_addNeiStatus(buzzvm_t vm){ int buzzuav_addNeiStatus(buzzvm_t vm){
buzzvm_lnum_assert(vm, 5); buzzvm_lnum_assert(vm, 5);
buzzvm_lload(vm, 1); // fc buzzvm_lload(vm, 1); // fc
buzzvm_lload(vm, 2); // xbee buzzvm_lload(vm, 2); // xbee
buzzvm_lload(vm, 3); // batt buzzvm_lload(vm, 3); // batt
buzzvm_lload(vm, 4); // gps buzzvm_lload(vm, 4); // gps
buzzvm_lload(vm, 5); // id buzzvm_lload(vm, 5); // id
buzzvm_type_assert(vm, 5, BUZZTYPE_INT); buzzvm_type_assert(vm, 5, BUZZTYPE_INT);
buzzvm_type_assert(vm, 4, BUZZTYPE_INT); buzzvm_type_assert(vm, 4, BUZZTYPE_INT);
buzzvm_type_assert(vm, 3, BUZZTYPE_INT); buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
buzzvm_type_assert(vm, 2, BUZZTYPE_INT); buzzvm_type_assert(vm, 2, BUZZTYPE_INT);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT); buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzz_utility::neighbors_status newRS; buzz_utility::neighbors_status newRS;
uint8_t id = buzzvm_stack_at(vm, 5)->i.value; uint8_t id = buzzvm_stack_at(vm, 5)->i.value;
newRS.gps_strenght= buzzvm_stack_at(vm, 4)->i.value; newRS.gps_strenght= buzzvm_stack_at(vm, 4)->i.value;
newRS.batt_lvl= buzzvm_stack_at(vm, 3)->i.value; newRS.batt_lvl= buzzvm_stack_at(vm, 3)->i.value;
newRS.xbee= buzzvm_stack_at(vm, 2)->i.value; newRS.xbee= buzzvm_stack_at(vm, 2)->i.value;
newRS.flight_status= buzzvm_stack_at(vm, 1)->i.value; newRS.flight_status= buzzvm_stack_at(vm, 1)->i.value;
map< int, buzz_utility::neighbors_status >::iterator it = neighbors_status_map.find(id); map< int, buzz_utility::neighbors_status >::iterator it = neighbors_status_map.find(id);
if(it!=neighbors_status_map.end()) if(it!=neighbors_status_map.end())
neighbors_status_map.erase(it); neighbors_status_map.erase(it);
neighbors_status_map.insert(make_pair(id, newRS)); neighbors_status_map.insert(make_pair(id, newRS));
@ -265,14 +265,17 @@ namespace buzzuav_closures{
return payload_out; return payload_out;
} }
/*----------------------------------------/ /*----------------------------------------/
/ Buzz closure to go directly to a GPS destination from the Mission Planner / Buzz closure to store locally a GPS destination from the fleet
/----------------------------------------*/ /----------------------------------------*/
int buzzuav_goto(buzzvm_t vm) { int buzzuav_storegoal(buzzvm_t vm) {
rc_goto_pos[2]=height; buzzvm_lnum_assert(vm, 3);
set_goto(rc_goto_pos); buzzvm_lload(vm, 1); // latitude
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT; buzzvm_lload(vm, 2); // longitude
printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0],goto_pos[1],goto_pos[2]); buzzvm_lload(vm, 3); // altitude
buzz_cmd=COMMAND_GOTO; buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
rc_set_goto(buzzvm_stack_at(vm, 1)->f.value, buzzvm_stack_at(vm, 2)->f.value, buzzvm_stack_at(vm, 3)->f.value, (int)buzz_utility::get_robotid());
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
@ -346,7 +349,7 @@ namespace buzzuav_closures{
return cmd; return cmd;
} }
void rc_set_goto(int id, double longitude, double latitude, double altitude) { void rc_set_goto(int id, double latitude, double longitude, double altitude) {
rc_id = id; rc_id = id;
rc_goto_pos[0] = latitude; rc_goto_pos[0] = latitude;
rc_goto_pos[1] = longitude; rc_goto_pos[1] = longitude;
@ -529,6 +532,10 @@ namespace buzzuav_closures{
buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "id", 1));
buzzvm_pushi(vm, rc_id);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
buzzvm_pushf(vm, rc_goto_pos[0]); buzzvm_pushf(vm, rc_goto_pos[0]);
buzzvm_tput(vm); buzzvm_tput(vm);

205
src/roscontroller.cpp
View File

@ -266,7 +266,8 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c)
/*Obtain .bzz file name from parameter server*/ /*Obtain .bzz file name from parameter server*/
if (n_c.getParam("bzzfile_name", bzzfile_name)) if (n_c.getParam("bzzfile_name", bzzfile_name))
; ;
else { else
{
ROS_ERROR("Provide a .bzz file to run in Launch file"); ROS_ERROR("Provide a .bzz file to run in Launch file");
system("rosnode kill rosbuzz_node"); system("rosnode kill rosbuzz_node");
} }
@ -284,13 +285,11 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c)
{ {
ROS_INFO("RC service is disabled"); ROS_INFO("RC service is disabled");
} }
} else { } else
{
ROS_ERROR("Provide a rc client option: yes or no in Launch file"); ROS_ERROR("Provide a rc client option: yes or no in Launch file");
system("rosnode kill rosbuzz_node"); system("rosnode kill rosbuzz_node");
} }
/*Obtain robot_id from parameter server*/
// n_c.getParam("robot_id", robot_id);
// robot_id=(int)buzz_utility::get_robotid();
/*Obtain out payload name*/ /*Obtain out payload name*/
n_c.getParam("out_payload", out_payload); n_c.getParam("out_payload", out_payload);
/*Obtain in payload name*/ /*Obtain in payload name*/
@ -300,22 +299,22 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c)
if (n_c.getParam("xbee_plugged", xbeeplugged)) if (n_c.getParam("xbee_plugged", xbeeplugged))
; ;
else { else
{
ROS_ERROR("Provide the xbee plugged boolean in Launch file"); ROS_ERROR("Provide the xbee plugged boolean in Launch file");
system("rosnode kill rosbuzz_node"); system("rosnode kill rosbuzz_node");
} }
if (!xbeeplugged) { if (!xbeeplugged) {
if (n_c.getParam("name", robot_name)) if (n_c.getParam("name", robot_name))
; ;
else { else
{
ROS_ERROR("Provide the xbee plugged boolean in Launch file"); ROS_ERROR("Provide the xbee plugged boolean in Launch file");
system("rosnode kill rosbuzz_node"); system("rosnode kill rosbuzz_node");
} }
} else } else
n_c.getParam("xbee_status_srv", xbeesrv_name); n_c.getParam("xbee_status_srv", xbeesrv_name);
std::cout << "////////////////// " << xbeesrv_name;
GetSubscriptionParameters(n_c); GetSubscriptionParameters(n_c);
// initialize topics to null? // initialize topics to null?
} }
@ -325,70 +324,71 @@ used
/-----------------------------------------------------------------------------------*/ /-----------------------------------------------------------------------------------*/
void roscontroller::GetSubscriptionParameters(ros::NodeHandle &node_handle) void roscontroller::GetSubscriptionParameters(ros::NodeHandle &node_handle)
{ {
// todo: make it as an array in yaml?
m_sMySubscriptions.clear(); m_sMySubscriptions.clear();
std::string gps_topic, gps_type; std::string gps_topic, gps_type;
if (node_handle.getParam("topics/gps", 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 Launch file");
system("rosnode kill rosbuzz_node"); system("rosnode kill rosbuzz_node");
} }
node_handle.getParam("type/gps", gps_type); m_smTopic_infos.insert(pair<std::string, std::string>(gps_topic, "sensor_msgs/NavSatFix"));
m_smTopic_infos.insert(pair<std::string, std::string>(gps_topic, gps_type));
std::string battery_topic, battery_type; std::string battery_topic;
node_handle.getParam("topics/battery", battery_topic); node_handle.getParam("topics/battery", battery_topic);
node_handle.getParam("type/battery", battery_type); m_smTopic_infos.insert(pair<std::string, std::string>(battery_topic, "mavros_msgs/BatteryState"));
m_smTopic_infos.insert(
pair<std::string, std::string>(battery_topic, battery_type));
std::string status_topic, status_type; std::string status_topic;
node_handle.getParam("topics/status", status_topic); node_handle.getParam("topics/status", status_topic);
node_handle.getParam("type/status", status_type); m_smTopic_infos.insert(pair<std::string, std::string>(status_topic, "mavros_msgs/State"));
m_smTopic_infos.insert( node_handle.getParam("topics/estatus", status_topic);
pair<std::string, std::string>(status_topic, status_type)); m_smTopic_infos.insert(pair<std::string, std::string>(status_topic, "mavros_msgs/ExtendedState"));
std::string altitude_topic, altitude_type; std::string altitude_topic;
node_handle.getParam("topics/altitude", altitude_topic); node_handle.getParam("topics/altitude", altitude_topic);
node_handle.getParam("type/altitude", altitude_type); m_smTopic_infos.insert(pair<std::string, std::string>(altitude_topic, "std_msgs/Float64"));
m_smTopic_infos.insert(
pair<std::string, std::string>(altitude_topic, altitude_type));
/*Obtain fc client name from 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))
; ;
else { else
{
ROS_ERROR("Provide a fc client name in Launch file"); ROS_ERROR("Provide a fc client name in Launch file");
system("rosnode kill rosbuzz_node"); system("rosnode kill rosbuzz_node");
} }
if (node_handle.getParam("topics/setpoint", setpoint_name)) if (node_handle.getParam("topics/setpoint", setpoint_name))
; ;
else { else
{
ROS_ERROR("Provide a Set Point name in Launch file"); ROS_ERROR("Provide a Set Point name in Launch file");
system("rosnode kill rosbuzz_node"); system("rosnode kill rosbuzz_node");
} }
if (node_handle.getParam("topics/armclient", armclient)) if (node_handle.getParam("topics/armclient", armclient))
; ;
else { else
{
ROS_ERROR("Provide an arm client name in Launch file"); ROS_ERROR("Provide an arm client name in Launch file");
system("rosnode kill rosbuzz_node"); system("rosnode kill rosbuzz_node");
} }
if (node_handle.getParam("topics/modeclient", modeclient)) if (node_handle.getParam("topics/modeclient", modeclient))
; ;
else { else
{
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/stream", stream_client_name)) if (node_handle.getParam("topics/stream", stream_client_name))
; ;
else { else
{
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)) if (node_handle.getParam("topics/localpos", local_pos_sub_name))
; ;
else { else
{
ROS_ERROR("Provide a localpos name in YAML file"); ROS_ERROR("Provide a localpos name in YAML file");
system("rosnode kill rosbuzz_node"); system("rosnode kill rosbuzz_node");
} }
@ -405,33 +405,28 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c)
Subscribe(n_c); Subscribe(n_c);
payload_sub = payload_sub = n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this);
n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this);
obstacle_sub = n_c.subscribe(obstacles_topic, 5, obstacle_sub = n_c.subscribe(obstacles_topic, 5, &roscontroller::obstacle_dist, this);
&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);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", 5); neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", 5);
localsetpoint_nonraw_pub = localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5);
n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5);
/* Service Clients*/ /* Service Clients*/
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient); arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient); mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name); mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
if (rcclient == true) if (rcclient == true)
service = service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this);
n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this);
ROS_INFO("Ready to receive Mav Commands from RC client"); ROS_INFO("Ready to receive Mav Commands from RC client");
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name); xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
stream_client = stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
users_sub = n_c.subscribe("users_pos", 5, &roscontroller::users_pos, this); users_sub = n_c.subscribe("users_pos", 5, &roscontroller::users_pos, this);
local_pos_sub = n_c.subscribe(local_pos_sub_name, 5, local_pos_sub = n_c.subscribe(local_pos_sub_name, 5, &roscontroller::local_pos_callback, this);
&roscontroller::local_pos_callback, this);
multi_msg = true; multi_msg = true;
} }
@ -444,20 +439,15 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c)
m_smTopic_infos.begin(); m_smTopic_infos.begin();
it != m_smTopic_infos.end(); ++it) { it != m_smTopic_infos.end(); ++it) {
if (it->second == "mavros_msgs/ExtendedState") { if (it->second == "mavros_msgs/ExtendedState") {
flight_status_sub = n_c.subscribe( flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_extended_status_update, this);
it->first, 100, &roscontroller::flight_extended_status_update, this);
} else if (it->second == "mavros_msgs/State") { } else if (it->second == "mavros_msgs/State") {
flight_status_sub = n_c.subscribe( flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_status_update, this);
it->first, 100, &roscontroller::flight_status_update, this);
} else if (it->second == "mavros_msgs/BatteryStatus") { } else if (it->second == "mavros_msgs/BatteryStatus") {
battery_sub = battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this);
n_c.subscribe(it->first, 5, &roscontroller::battery, this);
} else if (it->second == "sensor_msgs/NavSatFix") { } else if (it->second == "sensor_msgs/NavSatFix") {
current_position_sub = current_position_sub = n_c.subscribe(it->first, 5, &roscontroller::current_pos, this);
n_c.subscribe(it->first, 5, &roscontroller::current_pos, this);
} else if (it->second == "std_msgs/Float64") { } else if (it->second == "std_msgs/Float64") {
relative_altitude_sub = relative_altitude_sub = n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this);
n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this);
} }
std::cout << "Subscribed to: " << it->first << endl; std::cout << "Subscribed to: " << it->first << endl;
@ -1059,13 +1049,10 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) {
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];
double cvt_neighbours_pos_payload[3]; double cvt_neighbours_pos_payload[3];
// cout<<"Got" << neighbours_pos_payload[0] <<", " <<
//neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl;
gps_rb(nei_pos, cvt_neighbours_pos_payload); gps_rb(nei_pos, cvt_neighbours_pos_payload);
/*Extract robot id of the neighbour*/ /*Extract robot id of the neighbour*/
uint16_t *out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3)); uint16_t *out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3));
ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], //ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]);
cvt_neighbours_pos_payload[1]);
/*pass neighbour position to local maintaner*/ /*pass neighbour position to local maintaner*/
buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0], buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],
cvt_neighbours_pos_payload[1], cvt_neighbours_pos_payload[1],
@ -1089,60 +1076,60 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req,
mavros_msgs::CommandLong::Response &res) { mavros_msgs::CommandLong::Response &res) {
int rc_cmd; int rc_cmd;
switch (req.command) { switch (req.command) {
case mavros_msgs::CommandCode::NAV_TAKEOFF: case mavros_msgs::CommandCode::NAV_TAKEOFF:
ROS_INFO("RC_call: TAKE OFF!!!!"); ROS_INFO("RC_call: TAKE OFF!!!!");
rc_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF; rc_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_LAND:
ROS_INFO("RC_Call: LAND!!!! sending land");
rc_cmd = mavros_msgs::CommandCode::NAV_LAND;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM:
rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
armstate = req.param1;
if (armstate) {
ROS_INFO("RC_Call: ARM!!!!");
buzzuav_closures::rc_call(rc_cmd); buzzuav_closures::rc_call(rc_cmd);
res.success = true; res.success = true;
} else { break;
ROS_INFO("RC_Call: DISARM!!!!"); case mavros_msgs::CommandCode::NAV_LAND:
buzzuav_closures::rc_call(rc_cmd + 1); ROS_INFO("RC_Call: LAND!!!!");
rc_cmd = mavros_msgs::CommandCode::NAV_LAND;
buzzuav_closures::rc_call(rc_cmd);
res.success = true; res.success = true;
} break;
break; case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM:
case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH: rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
ROS_INFO("RC_Call: GO HOME!!!!"); armstate = req.param1;
rc_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; if (armstate) {
buzzuav_closures::rc_call(rc_cmd); ROS_INFO("RC_Call: ARM!!!!");
res.success = true; buzzuav_closures::rc_call(rc_cmd);
break; res.success = true;
case mavros_msgs::CommandCode::NAV_WAYPOINT: } else {
ROS_INFO("RC_Call: GO TO!!!! "); ROS_INFO("RC_Call: DISARM!!!!");
buzzuav_closures::rc_set_goto(req.param1,req.param5,req.param6,req.param7); buzzuav_closures::rc_call(rc_cmd + 1);
rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT; res.success = true;
buzzuav_closures::rc_call(rc_cmd); }
res.success = true; break;
break; case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL: ROS_INFO("RC_Call: GO HOME!!!!");
ROS_INFO("RC_Call: Gimbal!!!! "); rc_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
buzzuav_closures::rc_set_gimbal(req.param1,req.param2,req.param3); buzzuav_closures::rc_call(rc_cmd);
rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; res.success = true;
buzzuav_closures::rc_call(rc_cmd); break;
res.success = true; case mavros_msgs::CommandCode::NAV_WAYPOINT:
break; ROS_INFO("RC_Call: GO TO!!!! ");
case CMD_REQUEST_UPDATE: buzzuav_closures::rc_set_goto(req.param1,req.param5,req.param6,req.param7);
ROS_INFO("RC_Call: Update Fleet Status!!!!"); rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT;
rc_cmd = 666; buzzuav_closures::rc_call(rc_cmd);
buzzuav_closures::rc_call(rc_cmd); res.success = true;
res.success = true; break;
break; case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL:
default: ROS_INFO("RC_Call: Gimbal!!!! ");
res.success = false; buzzuav_closures::rc_set_gimbal(req.param1,req.param2,req.param3);
break; rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case CMD_REQUEST_UPDATE:
//ROS_INFO("RC_Call: Update Fleet Status!!!!");
rc_cmd = CMD_REQUEST_UPDATE;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
default:
res.success = false;
break;
} }
return true; return true;
} }