add gps goal control, re-structured launch dir and add height to buzz moveto closure
This commit is contained in:
dave
parent
5804aace94
commit
13267a9e97
|
|
@ -38,7 +38,7 @@ function action() {
|
|||
|
||||
# Move according to vector
|
||||
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"
|
||||
|
||||
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
|
||||
|
|
@ -47,11 +47,11 @@ function action() {
|
|||
# } else if(timeW>=WAIT_TIMEOUT/2) {
|
||||
# UAVSTATE ="GOEAST"
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(0.0,5.0)
|
||||
# uav_moveto(0.0, 5.0, 0.0)
|
||||
# } else {
|
||||
# UAVSTATE ="GONORTH"
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(5.0,0.0)
|
||||
# uav_moveto(5.0, 0.0, 0.0)
|
||||
# }
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -446,7 +446,7 @@ function TransitionToJoined(){
|
|||
|
||||
m_navigation.x=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.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
|
||||
if(math.vec2.length(m_navigation)>ROBOT_MAXVEL*2)
|
||||
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
|
||||
i=0
|
||||
var tempvec={.x=0.0,.y=0.0}
|
||||
|
|
@ -545,7 +545,7 @@ function DoFree() {
|
|||
i=i+1
|
||||
}
|
||||
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){
|
||||
m_navigation.x=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
|
||||
m_selfMessage.State=s2i(m_eState)
|
||||
|
|
@ -614,7 +614,7 @@ function DoAsking(){
|
|||
}
|
||||
m_navigation.x=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))
|
||||
|
||||
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.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
|
||||
|
|
@ -789,7 +789,7 @@ if(m_nLabel!=0){
|
|||
m_navigation=motion_vector()
|
||||
}
|
||||
#move
|
||||
uav_moveto(m_navigation.x,m_navigation.y)
|
||||
uav_moveto(m_navigation.x, m_navigation.y, 0.0)
|
||||
}
|
||||
|
||||
function action(){
|
||||
|
|
@ -912,7 +912,7 @@ function destroy() {
|
|||
#clear neighbour message
|
||||
m_navigation.x=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={}
|
||||
#stop listening
|
||||
neighbors.ignore("m")
|
||||
|
|
|
|||
|
|
@ -5,8 +5,10 @@
|
|||
########################################
|
||||
TARGET_ALTITUDE = 5.0
|
||||
UAVSTATE = "TURNEDOFF"
|
||||
GOTO_MAXVEL = 2
|
||||
goal = {.range=0, .bearing=0}
|
||||
|
||||
function uav_initswarm(){
|
||||
function uav_initswarm() {
|
||||
s = swarm.create(1)
|
||||
s.join()
|
||||
statef=turnedoff
|
||||
|
|
@ -26,13 +28,10 @@ function idle() {
|
|||
function takeoff() {
|
||||
UAVSTATE = "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) {
|
||||
barrier_set(ROBOTS,action,land)
|
||||
barrier_ready()
|
||||
#statef=hexagon
|
||||
}
|
||||
else {
|
||||
log("Altitude: ", TARGET_ALTITUDE)
|
||||
|
|
@ -44,7 +43,7 @@ function takeoff() {
|
|||
function land() {
|
||||
UAVSTATE = "LAND"
|
||||
statef=land
|
||||
#log("Land: ", flight.status)
|
||||
|
||||
if(flight.status == 2 or flight.status == 3){
|
||||
neighbors.broadcast("cmd", 21)
|
||||
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() {
|
||||
if(size(targets)>0) {
|
||||
UAVSTATE = "FOLLOW"
|
||||
|
|
@ -67,7 +88,7 @@ function follow() {
|
|||
force=(0.05)*(tab.range)^4
|
||||
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 {
|
||||
log("No target in local table!")
|
||||
#statef=idle
|
||||
|
|
@ -90,11 +111,8 @@ function uav_rccmd() {
|
|||
neighbors.broadcast("cmd", 21)
|
||||
} else if(flight.rc_cmd==16) {
|
||||
flight.rc_cmd=0
|
||||
UAVSTATE = "FOLLOW"
|
||||
log(rc_goto.latitude, " ", rc_goto.longitude)
|
||||
add_targetrb(0,rc_goto.latitude,rc_goto.longitude)
|
||||
statef = follow
|
||||
#uav_goto()
|
||||
UAVSTATE = "GOTO"
|
||||
statef = goto
|
||||
} else if(flight.rc_cmd==400) {
|
||||
flight.rc_cmd=0
|
||||
uav_arm()
|
||||
|
|
@ -114,13 +132,38 @@ function uav_neicmd() {
|
|||
function(vid, value, rid) {
|
||||
print("Got (", vid, ",", value, ") from robot #", rid)
|
||||
if(value==22 and UAVSTATE!="TAKEOFF") {
|
||||
statef=takeoff
|
||||
statef=takeoff
|
||||
} else if(value==21) {
|
||||
statef=land
|
||||
statef=land
|
||||
} else if(value==400 and UAVSTATE=="IDLE") {
|
||||
uav_arm()
|
||||
uav_arm()
|
||||
} 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));
|
||||
}
|
||||
|
|
@ -7,17 +7,22 @@ include "vstigenv.bzz"
|
|||
function action() {
|
||||
statef=action
|
||||
# test moveto cmd dx, dy
|
||||
# uav_moveto(0.5, 0.5)
|
||||
# uav_moveto(0.5, 0.5, 0.0)
|
||||
}
|
||||
|
||||
# Executed once at init time.
|
||||
function init() {
|
||||
uav_initswarm()
|
||||
uav_initstig()
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
function step() {
|
||||
log("Altitude: ", position.altitude)
|
||||
uav_rccmd()
|
||||
|
||||
statef()
|
||||
|
||||
log("Current state: ", UAVSTATE)
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@ int buzzros_print(buzzvm_t vm);
|
|||
* commands the UAV to go to a position supplied
|
||||
*/
|
||||
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*/
|
||||
int getcmd();
|
||||
/*Sets goto position */
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
@ -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
|
||||
|
|
@ -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>
|
||||
|
|
@ -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>
|
||||
|
|
@ -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>
|
||||
|
|
@ -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>
|
||||
<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/src/testmoveto.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="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
|
||||
</node>
|
||||
<arg name="name" default="robot0"/>
|
||||
<arg name="xbee_plugged" default="true"/>
|
||||
<arg name="script" default="testalone"/>
|
||||
|
||||
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" launch-prefix="gdb -ex run --args">
|
||||
<rosparam file="$(find rosbuzz)/launch/launch_config/topics.yaml"/>
|
||||
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).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="$(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>
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
@ -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>
|
||||
|
|
@ -304,8 +304,8 @@ void in_message_process(){
|
|||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto));
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal));
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
|
||||
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_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
|
||||
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_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
|
||||
|
|
|
|||
|
|
@ -123,27 +123,27 @@ namespace buzzuav_closures{
|
|||
/ Buzz closure to move following a 2D vector
|
||||
/----------------------------------------*/
|
||||
int buzzuav_moveto(buzzvm_t vm) {
|
||||
buzzvm_lnum_assert(vm, 2);
|
||||
buzzvm_lload(vm, 1); /* dx */
|
||||
buzzvm_lload(vm, 2); /* dy */
|
||||
//buzzvm_lload(vm, 3); /* Latitude */
|
||||
//buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
|
||||
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
|
||||
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
|
||||
float dy = buzzvm_stack_at(vm, 1)->f.value;
|
||||
float dx = buzzvm_stack_at(vm, 2)->f.value;
|
||||
double d = sqrt(dx*dx+dy*dy); //range
|
||||
goto_pos[0]=dx;
|
||||
goto_pos[1]=dy;
|
||||
goto_pos[2]=height;
|
||||
/*double b = atan2(dy,dx); //bearing
|
||||
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
|
||||
gps_from_rb(d, b, goto_pos);
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
|
||||
//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]);
|
||||
buzz_cmd= COMMAND_MOVETO; // TO DO what should we use
|
||||
return buzzvm_ret0(vm);
|
||||
buzzvm_lnum_assert(vm, 3);
|
||||
buzzvm_lload(vm, 1); /* dx */
|
||||
buzzvm_lload(vm, 2); /* dy */
|
||||
buzzvm_lload(vm, 3); /* dheight */
|
||||
buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
|
||||
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
|
||||
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
|
||||
float dh = buzzvm_stack_at(vm, 1)->f.value;
|
||||
float dy = buzzvm_stack_at(vm, 2)->f.value;
|
||||
float dx = buzzvm_stack_at(vm, 3)->f.value;
|
||||
goto_pos[0]=dx;
|
||||
goto_pos[1]=dy;
|
||||
goto_pos[2]=height+dh;
|
||||
/*double b = atan2(dy,dx); //bearing
|
||||
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
|
||||
gps_from_rb(d, b, goto_pos);
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
|
||||
//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]);
|
||||
buzz_cmd= COMMAND_MOVETO; // TO DO what should we use
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
int buzzuav_update_targets(buzzvm_t vm) {
|
||||
|
|
@ -224,23 +224,23 @@ namespace buzzuav_closures{
|
|||
|
||||
int buzzuav_addNeiStatus(buzzvm_t vm){
|
||||
buzzvm_lnum_assert(vm, 5);
|
||||
buzzvm_lload(vm, 1); // fc
|
||||
buzzvm_lload(vm, 2); // xbee
|
||||
buzzvm_lload(vm, 3); // batt
|
||||
buzzvm_lload(vm, 4); // gps
|
||||
buzzvm_lload(vm, 5); // id
|
||||
buzzvm_type_assert(vm, 5, BUZZTYPE_INT);
|
||||
buzzvm_type_assert(vm, 4, BUZZTYPE_INT);
|
||||
buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
|
||||
buzzvm_type_assert(vm, 2, BUZZTYPE_INT);
|
||||
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
|
||||
buzzvm_lload(vm, 1); // fc
|
||||
buzzvm_lload(vm, 2); // xbee
|
||||
buzzvm_lload(vm, 3); // batt
|
||||
buzzvm_lload(vm, 4); // gps
|
||||
buzzvm_lload(vm, 5); // id
|
||||
buzzvm_type_assert(vm, 5, BUZZTYPE_INT);
|
||||
buzzvm_type_assert(vm, 4, BUZZTYPE_INT);
|
||||
buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
|
||||
buzzvm_type_assert(vm, 2, BUZZTYPE_INT);
|
||||
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
|
||||
buzz_utility::neighbors_status newRS;
|
||||
uint8_t id = buzzvm_stack_at(vm, 5)->i.value;
|
||||
newRS.gps_strenght= buzzvm_stack_at(vm, 4)->i.value;
|
||||
newRS.batt_lvl= buzzvm_stack_at(vm, 3)->i.value;
|
||||
newRS.xbee= buzzvm_stack_at(vm, 2)->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);
|
||||
newRS.gps_strenght= buzzvm_stack_at(vm, 4)->i.value;
|
||||
newRS.batt_lvl= buzzvm_stack_at(vm, 3)->i.value;
|
||||
newRS.xbee= buzzvm_stack_at(vm, 2)->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);
|
||||
if(it!=neighbors_status_map.end())
|
||||
neighbors_status_map.erase(it);
|
||||
neighbors_status_map.insert(make_pair(id, newRS));
|
||||
|
|
@ -265,14 +265,17 @@ namespace buzzuav_closures{
|
|||
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) {
|
||||
rc_goto_pos[2]=height;
|
||||
set_goto(rc_goto_pos);
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
|
||||
printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0],goto_pos[1],goto_pos[2]);
|
||||
buzz_cmd=COMMAND_GOTO;
|
||||
int buzzuav_storegoal(buzzvm_t vm) {
|
||||
buzzvm_lnum_assert(vm, 3);
|
||||
buzzvm_lload(vm, 1); // latitude
|
||||
buzzvm_lload(vm, 2); // longitude
|
||||
buzzvm_lload(vm, 3); // altitude
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -346,7 +349,7 @@ namespace buzzuav_closures{
|
|||
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_goto_pos[0] = latitude;
|
||||
rc_goto_pos[1] = longitude;
|
||||
|
|
@ -529,6 +532,10 @@ namespace buzzuav_closures{
|
|||
buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1));
|
||||
buzzvm_pusht(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_pushf(vm, rc_goto_pos[0]);
|
||||
buzzvm_tput(vm);
|
||||
|
|
|
|||
|
|
@ -266,7 +266,8 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c)
|
|||
/*Obtain .bzz file name from parameter server*/
|
||||
if (n_c.getParam("bzzfile_name", bzzfile_name))
|
||||
;
|
||||
else {
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Provide a .bzz file to run in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
|
|
@ -284,13 +285,11 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c)
|
|||
{
|
||||
ROS_INFO("RC service is disabled");
|
||||
}
|
||||
} else {
|
||||
} else
|
||||
{
|
||||
ROS_ERROR("Provide a rc client option: yes or no in Launch file");
|
||||
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*/
|
||||
n_c.getParam("out_payload", out_payload);
|
||||
/*Obtain in payload name*/
|
||||
|
|
@ -300,22 +299,22 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c)
|
|||
|
||||
if (n_c.getParam("xbee_plugged", xbeeplugged))
|
||||
;
|
||||
else {
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Provide the xbee plugged boolean in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
if (!xbeeplugged) {
|
||||
if (n_c.getParam("name", robot_name))
|
||||
;
|
||||
else {
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Provide the xbee plugged boolean in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
} else
|
||||
n_c.getParam("xbee_status_srv", xbeesrv_name);
|
||||
|
||||
std::cout << "////////////////// " << xbeesrv_name;
|
||||
|
||||
GetSubscriptionParameters(n_c);
|
||||
// initialize topics to null?
|
||||
}
|
||||
|
|
@ -325,70 +324,71 @@ used
|
|||
/-----------------------------------------------------------------------------------*/
|
||||
void roscontroller::GetSubscriptionParameters(ros::NodeHandle &node_handle)
|
||||
{
|
||||
// todo: make it as an array in yaml?
|
||||
m_sMySubscriptions.clear();
|
||||
std::string gps_topic, gps_type;
|
||||
if (node_handle.getParam("topics/gps", gps_topic))
|
||||
;
|
||||
else {
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Provide a gps topic in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
node_handle.getParam("type/gps", gps_type);
|
||||
m_smTopic_infos.insert(pair<std::string, std::string>(gps_topic, gps_type));
|
||||
m_smTopic_infos.insert(pair<std::string, std::string>(gps_topic, "sensor_msgs/NavSatFix"));
|
||||
|
||||
std::string battery_topic, battery_type;
|
||||
std::string 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, battery_type));
|
||||
m_smTopic_infos.insert(pair<std::string, std::string>(battery_topic, "mavros_msgs/BatteryState"));
|
||||
|
||||
std::string status_topic, status_type;
|
||||
std::string 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, status_type));
|
||||
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"));
|
||||
|
||||
std::string altitude_topic, altitude_type;
|
||||
std::string 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, altitude_type));
|
||||
m_smTopic_infos.insert(pair<std::string, std::string>(altitude_topic, "std_msgs/Float64"));
|
||||
|
||||
/*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))
|
||||
;
|
||||
else {
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Provide a fc client name in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
if (node_handle.getParam("topics/setpoint", setpoint_name))
|
||||
;
|
||||
else {
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Provide a Set Point name in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
if (node_handle.getParam("topics/armclient", armclient))
|
||||
;
|
||||
else {
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Provide an arm client name in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
if (node_handle.getParam("topics/modeclient", modeclient))
|
||||
;
|
||||
else {
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Provide a mode client name in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
if (node_handle.getParam("topics/stream", stream_client_name))
|
||||
;
|
||||
else {
|
||||
else
|
||||
{
|
||||
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 {
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Provide a localpos name in YAML file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
|
|
@ -405,33 +405,28 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c)
|
|||
|
||||
Subscribe(n_c);
|
||||
|
||||
payload_sub =
|
||||
n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this);
|
||||
payload_sub = n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this);
|
||||
|
||||
obstacle_sub = n_c.subscribe(obstacles_topic, 5,
|
||||
&roscontroller::obstacle_dist, 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);
|
||||
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", 5);
|
||||
localsetpoint_nonraw_pub =
|
||||
n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5);
|
||||
localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5);
|
||||
|
||||
/* Service Clients*/
|
||||
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
|
||||
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
|
||||
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
|
||||
if (rcclient == true)
|
||||
service =
|
||||
n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this);
|
||||
service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this);
|
||||
ROS_INFO("Ready to receive Mav Commands from RC client");
|
||||
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
|
||||
stream_client =
|
||||
n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
|
||||
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
|
||||
|
||||
users_sub = n_c.subscribe("users_pos", 5, &roscontroller::users_pos, this);
|
||||
local_pos_sub = n_c.subscribe(local_pos_sub_name, 5,
|
||||
&roscontroller::local_pos_callback, this);
|
||||
local_pos_sub = n_c.subscribe(local_pos_sub_name, 5, &roscontroller::local_pos_callback, this);
|
||||
|
||||
multi_msg = true;
|
||||
}
|
||||
|
|
@ -444,20 +439,15 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c)
|
|||
m_smTopic_infos.begin();
|
||||
it != m_smTopic_infos.end(); ++it) {
|
||||
if (it->second == "mavros_msgs/ExtendedState") {
|
||||
flight_status_sub = n_c.subscribe(
|
||||
it->first, 100, &roscontroller::flight_extended_status_update, this);
|
||||
flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_extended_status_update, this);
|
||||
} else if (it->second == "mavros_msgs/State") {
|
||||
flight_status_sub = n_c.subscribe(
|
||||
it->first, 100, &roscontroller::flight_status_update, this);
|
||||
flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_status_update, this);
|
||||
} else if (it->second == "mavros_msgs/BatteryStatus") {
|
||||
battery_sub =
|
||||
n_c.subscribe(it->first, 5, &roscontroller::battery, this);
|
||||
battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this);
|
||||
} 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::current_pos, 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::current_rel_alt, this);
|
||||
}
|
||||
|
||||
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.altitude = neighbours_pos_payload[2];
|
||||
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);
|
||||
/*Extract robot id of the neighbour*/
|
||||
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],
|
||||
cvt_neighbours_pos_payload[1]);
|
||||
//ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]);
|
||||
/*pass neighbour position to local maintaner*/
|
||||
buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],
|
||||
cvt_neighbours_pos_payload[1],
|
||||
|
|
@ -1089,60 +1076,60 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req,
|
|||
mavros_msgs::CommandLong::Response &res) {
|
||||
int rc_cmd;
|
||||
switch (req.command) {
|
||||
case mavros_msgs::CommandCode::NAV_TAKEOFF:
|
||||
ROS_INFO("RC_call: TAKE OFF!!!!");
|
||||
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!!!!");
|
||||
case mavros_msgs::CommandCode::NAV_TAKEOFF:
|
||||
ROS_INFO("RC_call: TAKE OFF!!!!");
|
||||
rc_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF;
|
||||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
} else {
|
||||
ROS_INFO("RC_Call: DISARM!!!!");
|
||||
buzzuav_closures::rc_call(rc_cmd + 1);
|
||||
break;
|
||||
case mavros_msgs::CommandCode::NAV_LAND:
|
||||
ROS_INFO("RC_Call: LAND!!!!");
|
||||
rc_cmd = mavros_msgs::CommandCode::NAV_LAND;
|
||||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
}
|
||||
break;
|
||||
case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
|
||||
ROS_INFO("RC_Call: GO HOME!!!!");
|
||||
rc_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
|
||||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
break;
|
||||
case mavros_msgs::CommandCode::NAV_WAYPOINT:
|
||||
ROS_INFO("RC_Call: GO TO!!!! ");
|
||||
buzzuav_closures::rc_set_goto(req.param1,req.param5,req.param6,req.param7);
|
||||
rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT;
|
||||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
break;
|
||||
case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL:
|
||||
ROS_INFO("RC_Call: Gimbal!!!! ");
|
||||
buzzuav_closures::rc_set_gimbal(req.param1,req.param2,req.param3);
|
||||
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 = 666;
|
||||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
break;
|
||||
default:
|
||||
res.success = false;
|
||||
break;
|
||||
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);
|
||||
res.success = true;
|
||||
} else {
|
||||
ROS_INFO("RC_Call: DISARM!!!!");
|
||||
buzzuav_closures::rc_call(rc_cmd + 1);
|
||||
res.success = true;
|
||||
}
|
||||
break;
|
||||
case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
|
||||
ROS_INFO("RC_Call: GO HOME!!!!");
|
||||
rc_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
|
||||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
break;
|
||||
case mavros_msgs::CommandCode::NAV_WAYPOINT:
|
||||
ROS_INFO("RC_Call: GO TO!!!! ");
|
||||
buzzuav_closures::rc_set_goto(req.param1,req.param5,req.param6,req.param7);
|
||||
rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT;
|
||||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
break;
|
||||
case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL:
|
||||
ROS_INFO("RC_Call: Gimbal!!!! ");
|
||||
buzzuav_closures::rc_set_gimbal(req.param1,req.param2,req.param3);
|
||||
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;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue