topic change for buzz objects
This commit is contained in:
parent
eaf3cf4f33
commit
60aa13a4ad
|
@ -2,22 +2,23 @@
|
||||||
|
|
||||||
<launch>
|
<launch>
|
||||||
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
|
<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/test.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_node1/outMavlink"/>
|
|
||||||
<param name="out_payload" value="outMavlink"/>
|
|
||||||
<param name="robot_id" value="1"/>
|
|
||||||
</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="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/test1.bzz" />
|
||||||
<param name="rcclient" value="true" />
|
<param name="rcclient" value="true" />
|
||||||
<param name="rcservice_name" value="rc_cmd" />
|
<param name="rcservice_name" value="rc_cmd" />
|
||||||
<param name="fcclient_name" value="/djicmd" />
|
<param name="fcclient_name" value="/djicmd" />
|
||||||
<param name="in_payload" value="/rosbuzz_node/outMavlink"/>
|
<param name="in_payload" value="/inMavlink"/>
|
||||||
<param name="out_payload" value="outMavlink"/>
|
<!-- param name="in_payload" value="/rosbuzz_node1/outMavlink"-->
|
||||||
<param name="robot_id" value="2"/>
|
<param name="out_payload" value="/outMavlink"/>
|
||||||
|
<param name="robot_id" value="1"/>
|
||||||
</node>
|
</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>
|
</launch>
|
||||||
|
|
|
@ -86,10 +86,10 @@ namespace rosbzz_node{
|
||||||
|
|
||||||
void roscontroller::Initialize_pub_sub(ros::NodeHandle n_c){
|
void roscontroller::Initialize_pub_sub(ros::NodeHandle n_c){
|
||||||
/*subscribers*/
|
/*subscribers*/
|
||||||
current_position_sub = n_c.subscribe("/mav/global_position", 1000, &roscontroller::current_pos,this);
|
current_position_sub = n_c.subscribe("/global_position", 1000, &roscontroller::current_pos,this);
|
||||||
battery_sub = n_c.subscribe("/mav/power_status", 1000, &roscontroller::battery,this);
|
battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this);
|
||||||
payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this);
|
payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this);
|
||||||
flight_status_sub =n_c.subscribe("/mav/flight_status",100, &roscontroller::flight_status_update,this);
|
flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_status_update,this);
|
||||||
/*publishers*/
|
/*publishers*/
|
||||||
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
|
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
|
||||||
cout<<out_payload<<endl;
|
cout<<out_payload<<endl;
|
||||||
|
|
|
@ -1,9 +1,86 @@
|
||||||
|
|
||||||
|
########################################
|
||||||
|
#
|
||||||
|
# BARRIER-RELATED FUNCTIONS
|
||||||
|
#
|
||||||
|
########################################
|
||||||
|
|
||||||
|
#
|
||||||
|
# Constants
|
||||||
|
#
|
||||||
|
BARRIER_VSTIG = 1
|
||||||
|
ROBOTS = 2 # number of robots in the swarm
|
||||||
|
|
||||||
|
#
|
||||||
|
# Sets a barrier
|
||||||
|
#
|
||||||
|
function barrier_set(threshold, transf) {
|
||||||
|
statef = function() {
|
||||||
|
barrier_wait(threshold, transf);
|
||||||
|
}
|
||||||
|
barrier = stigmergy.create(BARRIER_VSTIG)
|
||||||
|
statestr = "barrier"
|
||||||
|
}
|
||||||
|
|
||||||
|
#
|
||||||
|
# Make yourself ready
|
||||||
|
#
|
||||||
|
function barrier_ready() {
|
||||||
|
barrier.put(id, 1)
|
||||||
|
}
|
||||||
|
|
||||||
|
#
|
||||||
|
# Executes the barrier
|
||||||
|
#
|
||||||
|
function barrier_wait(threshold, transf) {
|
||||||
|
barrier.get(id)
|
||||||
|
extradbg = barrier.size()
|
||||||
|
if(barrier.size() >= threshold) {
|
||||||
|
barrier = nil
|
||||||
|
transf()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
# flight status
|
||||||
|
|
||||||
|
function idle() {
|
||||||
|
}
|
||||||
|
|
||||||
|
function takeoff() {
|
||||||
|
if( flight.status == 2) {
|
||||||
|
barrier_set(ROBOTS,transition_to_land)
|
||||||
|
barrier_ready()
|
||||||
|
}
|
||||||
|
else
|
||||||
|
uav_takeoff()
|
||||||
|
}
|
||||||
|
function land() {
|
||||||
|
if( flight.status == 1) {
|
||||||
|
barrier_set(ROBOTS,idle)
|
||||||
|
barrier_ready()
|
||||||
|
}
|
||||||
|
else
|
||||||
|
uav_land()
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
function transition_to_land() {
|
||||||
|
if(tim>=100){
|
||||||
|
statef=land
|
||||||
|
tim=0
|
||||||
|
}else{
|
||||||
|
tim=tim+1
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
# Executed once at init time.
|
# Executed once at init time.
|
||||||
function init() {
|
function init() {
|
||||||
i = 0
|
i = 0
|
||||||
a = 0
|
a = 0
|
||||||
val = 0
|
val = 0
|
||||||
oldcmd = 0
|
oldcmd = 0
|
||||||
|
tim=0
|
||||||
|
statef=idle
|
||||||
}
|
}
|
||||||
|
|
||||||
# Executed at each time step.
|
# Executed at each time step.
|
||||||
|
@ -13,25 +90,28 @@ neighbors.listen("cmd",
|
||||||
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) {
|
if(value==22) {
|
||||||
uav_takeoff()
|
statef=takeoff
|
||||||
} else if(value==21) {
|
} else if(value==21) {
|
||||||
uav_land()
|
statef=land
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
print("Flight status: ",flight.status)
|
||||||
if(flight.rc_cmd!=oldcmd) {
|
if(flight.rc_cmd!=oldcmd) {
|
||||||
print(flight.rc_cmd)
|
|
||||||
if(flight.rc_cmd==22) {
|
if(flight.rc_cmd==22) {
|
||||||
uav_takeoff()
|
statef=takeoff
|
||||||
neighbors.broadcast("cmd", 22)
|
neighbors.broadcast("cmd", 22)
|
||||||
} else if(flight.rc_cmd==21) {
|
} else if(flight.rc_cmd==21) {
|
||||||
uav_land()
|
statef=land
|
||||||
neighbors.broadcast("cmd", 21)
|
neighbors.broadcast("cmd", 21)
|
||||||
}
|
}
|
||||||
oldcmd=flight.rc_cmd
|
oldcmd=flight.rc_cmd
|
||||||
}
|
}
|
||||||
|
|
||||||
|
statef()
|
||||||
}
|
}
|
||||||
|
|
||||||
# Executed once when the robot (or the simulator) is reset.
|
# Executed once when the robot (or the simulator) is reset.
|
||||||
|
|
Loading…
Reference in New Issue