From 32c10dbc42497d12f7b3dc57a2aab0f601d280b5 Mon Sep 17 00:00:00 2001 From: dave Date: Wed, 30 Aug 2017 14:58:44 -0400 Subject: [PATCH] fix topic naming and rosbuzz launch --- buzz_scripts/flock.bzz | 7 +- buzz_scripts/graphform.bzz | 29 +- buzz_scripts/graphform.bzz_old | 826 ----------------------------- buzz_scripts/include/barrier.bzz | 12 +- buzz_scripts/include/uavstates.bzz | 39 +- buzz_scripts/testalone.bzz | 3 +- include/roscontroller.h | 1 + launch/launch_config/husky.yaml | 17 - launch/launch_config/m100.yaml | 17 - launch/launch_config/solo.yaml | 21 - launch/launch_config/topics.yaml | 2 +- launch/rosbuzz.launch | 2 +- launch/rosbuzzd.launch | 22 + src/buzz_utility.cpp | 10 +- src/roscontroller.cpp | 26 +- 15 files changed, 87 insertions(+), 947 deletions(-) delete mode 100644 buzz_scripts/graphform.bzz_old delete mode 100644 launch/launch_config/husky.yaml delete mode 100644 launch/launch_config/m100.yaml delete mode 100644 launch/launch_config/solo.yaml create mode 100644 launch/rosbuzzd.launch diff --git a/buzz_scripts/flock.bzz b/buzz_scripts/flock.bzz index bb2da81..b8de0cf 100644 --- a/buzz_scripts/flock.bzz +++ b/buzz_scripts/flock.bzz @@ -65,16 +65,19 @@ function action() { function init() { uav_initswarm() uav_initstig() - TARGET_ALTITUDE = 2.5 + id * 5 + # TARGET_ALTITUDE = 2.5 + id * 5 + statef=turnedoff + UAVSTATE = "TURNEDOFF" } # Executed at each time step. function step() { uav_rccmd() uav_neicmd() + uav_updatestig() statef() - uav_updatestig() + log("Current state: ", UAVSTATE) log("Swarm size: ",ROBOTS) if(id==0) diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz index ae0bd5b..7382af9 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -10,9 +10,10 @@ include "vstigenv.bzz" include "graphs/shapes_Y.bzz" -ROBOT_RADIUS=50 -ROBOT_DIAMETER=2.0*ROBOT_RADIUS -ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER +ROBOT_RADIUS = 50 +ROBOT_DIAMETER = 2.0*ROBOT_RADIUS +ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER +ROOT_ID = 2 # max velocity in cm/step ROBOT_MAXVEL = 250.0 @@ -802,7 +803,7 @@ function action(){ # function init() { # - #Adjust parameters here + # Global parameters for graph formation # m_unResponseTimeThreshold=10 m_unLabelSearchWaitTime=10 @@ -816,23 +817,32 @@ function init() { uav_initswarm() v_tag = stigmergy.create(m_lockstig) uav_initstig() - Reset() + # go to diff. height since no collision avoidance implemented yet TARGET_ALTITUDE = 2.5 + id * 1.5 + statef=turnedoff + UAVSTATE = "TURNEDOFF" + + # reset the graph + Reset() } # # Executed every step # function step(){ + # listen to potential RC uav_rccmd() + # get the swarm commands uav_neicmd() + # update the vstig (status/net/batt) uav_updatestig() + #update the graph UpdateNodeInfo() #reset message package to be sent m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} # - #act according to current state + # graph state machine # if(UAVSTATE=="GRAPH"){ if(m_eState=="STATE_FREE") @@ -849,17 +859,16 @@ function step(){ DoLock() } + # high level UAV state machine statef() debug(m_eState,m_nLabel) log("Current state: ", UAVSTATE) log("Swarm size: ", ROBOTS) -# if(id==0) -# stattab_print() #navigation - #broadcast messag + #broadcast message neighbors.broadcast("m",packmessage(m_selfMessage)) # @@ -895,7 +904,7 @@ function Reset(){ #set initial state, only one robot choose [A], while the rest choose [B] # #[A]The robot used to triger the formation process is defined as joined, - if(id==0){ + if(id==ROOT_ID){ m_nLabel=0 TransitionToJoined() } diff --git a/buzz_scripts/graphform.bzz_old b/buzz_scripts/graphform.bzz_old deleted file mode 100644 index 937d69c..0000000 --- a/buzz_scripts/graphform.bzz_old +++ /dev/null @@ -1,826 +0,0 @@ -# -# Include files -# -include "string.bzz" -include "vec2.bzz" -include "update.bzz" -include "barrier.bzz" # don't use a stigmergy id=11 with this header. -include "uavstates.bzz" # require an 'action' function to be defined here. - -ROBOT_RADIUS=50 -ROBOT_DIAMETER=2.0*ROBOT_RADIUS -ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER - -# -# Global variables -# - -# -#Save message from all neighours -#the indexes are as 1,2,3..., while each value is a table that store the information of a neighbour robot -m_MessageState={}#store received neighbour message -m_MessageLable={}#store received neighbour message -m_MessageReqLable={}#store received neighbour message -m_MessageReqID={}#store received neighbour message -m_MessageSide={}#store received neighbour message -m_MessageResponse={}#store received neighbour message -m_MessageRange={}#store received neighbour message -m_MessageBearing={}#store received neighbour message -m_neighbourCunt=0#used to cunt neighbours -#Save message from one neighbour -#the indexes are as State(received state),Lable(received lable),ReqLable,ReqID,Side,Response,Range,Bearing -m_receivedMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE",.Range=0,.Bearing=0} -# -#Save the message to send -#The keys of the talbe is State(current state),Lable(current lable),ReqLable(requested lable),ReqID(request id),Side(side),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND}) -m_selfMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE"} - -#Current robot state -m_eState="STATE_FREE" - -#navigation vector -m_navigation={.x=0,.y=0} - -#Debug message to be displayed in qt-opengl -#m_ossDebugMsg - -#Debug vector to draw -#CVector2 m_cDebugVector - -#Table of the nodes in the graph -m_vecNodes={} -m_vecNodes_fixed={} - -#Current label being requested or chosen (-1 when none) -m_nLabel=-1 - -#Label request id -m_unRequestId=0 - -#Side -m_unbSide=0 - -#Global bias, used to map local coordinate to global coordinate -m_bias=0 - -#Vector to predecessor,range is the distance between robots, bearing is the angle of pred wrt self in local coordinate of self, globalbearing is the angle of self wrt pred in global coordinate -m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0} - -#Counter to wait for something to happen -m_unWaitCount=0 - -#Number of steps to wait before looking for a free label -m_unLabelSearchWaitTime=0 - -#Number of steps to wait for an answer to be received -m_unResponseTimeThreshold=0 - -#Number of steps to wait until giving up joining -m_unJoiningLostPeriod=0 - -#Tolerance distance to a target location -m_fTargetDistanceTolerance=0 - -#step cunt -step_cunt=0 - -#virtual stigmergy -v_tag = stigmergy.create(1) - -# Lennard-Jones parameters -EPSILON = 13.5 #3.5 - -# Lennard-Jones interaction magnitude - -function FlockInteraction(dist,target,epsilon){ - var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) - return mag -} - -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 -} - -# -# Calculates the angle of the given vector2. -# PARAM v: The vector2. -# RETURN: The angle of the vector. -# -Angle = function(v) { - return math.atan(v.y, v.x) -} - -# -#return the number of value in table -# -function count(table,value){ - var number=0 - var i=0 - while(i0) and (m_vecNodes[i].State=="ASSIGNING")){ - m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1 - if(m_vecNodes[i].StateAge==0) - m_vecNodes[i].State="UNASSIGNED" - } - i=i+1 - } -} - -# -#Transistion to state free -# -function TransitionToFree(){ - m_eState="STATE_FREE" - m_unWaitCount=m_unLabelSearchWaitTime - m_selfMessage.State=m_eState -} - -# -#Transistion to state asking -# -function TransitionToAsking(un_label){ - m_eState="STATE_ASKING" - m_nLabel=un_label - m_unRequestId=rng.uniform(0,65536)+id#don't know why the random numbers are the same, add id to make the ReqID different - m_selfMessage.State=m_eState - m_selfMessage.ReqLable=m_nLabel - m_selfMessage.ReqID=m_unRequestId - - m_unWaitCount=m_unResponseTimeThreshold -} - -# -#Transistion to state joining -# -function TransitionToJoining(){ - m_eState="STATE_JOINING" - m_selfMessage.State=m_eState - m_selfMessage.Lable=m_nLabel - m_unWaitCount=m_unJoiningLostPeriod - - neighbors.listen("reply", - function(vid,value,rid){ - #store the received message - if(value.Lable==m_nLabel){ - m_cMeToPred.GlobalBearing=value.GlobalBearing - - } - }) - -} - -# -#Transistion to state joined -# -function TransitionToJoined(){ - m_eState="STATE_JOINED" - m_selfMessage.State=m_eState - m_selfMessage.Lable=m_nLabel - m_vecNodes[m_nLabel].State="ASSIGNED" - neighbors.ignore("reply") - - #write statues - v_tag.put(m_nLabel, 1) - - m_navigation.x=0.0 - m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) -} - -# -#Transistion to state Lock, lock the current formation -# -function TransitionToLock(){ - m_eState="STATE_LOCK" - m_selfMessage.State=m_eState - m_selfMessage.Lable=m_nLabel - m_vecNodes[m_nLabel].State="ASSIGNED" - - m_navigation.x=0.0 - m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) -} - -# -# Do free -# -function DoFree() { - m_selfMessage.State=m_eState - #wait for a while before looking for a lable - if(m_unWaitCount>0) - m_unWaitCount=m_unWaitCount-1 - - #find a set of joined robots - var setJoinedLables={} - var setJoinedIndexes={} - var i=0 - var j=0 - while(i0){ - TransitionToAsking(unFoundLable) - return - } - - #navigation - #if there is a joined robot within sight, move around joined robots - #else, gather with other free robots - if(size(setJoinedIndexes)>0){ - var tempvec_P={.x=0.0,.y=0.0} - var tempvec_N={.x=0.0,.y=0.0} - i=0 - while(imath.pi) - S2PGlobalBearing=m_cMeToPred.GlobalBearing-math.pi - else - S2PGlobalBearing=m_cMeToPred.GlobalBearing+math.pi - - var S2Pred=math.vec2.newp(m_cMeToPred.Range,S2PGlobalBearing) - - #the vector from self to target in global coordinate - var S2Target=math.vec2.add(S2Pred,P2Target) - #change the vector to local coordinate of self - var S2Target_bearing=Angle(S2Target) - m_bias=m_cMeToPred.Bearing-S2PGlobalBearing - #S2Target_bearing=S2Target_bearing+m_bias # commented out by DS'06/17 - m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing) - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) - - - - #test if is already in desired position - if(math.abs(S2Target.x)m_MessageRange[mapRequests[i]]) - ReqIndex=i - i=i+1 - } - #get the best index, whose Reqlable and Reqid are - ReqLable=m_MessageReqLable[mapRequests[ReqIndex]] - var ReqID=m_MessageReqID[mapRequests[ReqIndex]] - m_selfMessage.ReqLable=ReqLable - m_selfMessage.ReqID=ReqID - m_selfMessage.Response="REQ_GRANTED" - } - - #lost pred, wait for some time and transit to free - #if(seenPred==0){ - #m_unWaitCount=m_unWaitCount-1 - #if(m_unWaitCount==0){ - #TransitionToFree() - #return - #} - #} - - - m_navigation.x=0.0 - m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) - - - #check if should to transists to lock - - - if(v_tag.size()==ROBOTS){ - TransitionToLock() - } -} - -# -#Do Lock -# -function DoLock(){ -m_selfMessage.State=m_eState -m_selfMessage.Lable=m_nLabel - -m_navigation.x=0.0 -m_navigation.y=0.0 - -#collect preds information -var i=0 -var mypred1={.range=0,.bearing=0} -var mypred2={.range=0,.bearing=0} - -while(i1){ - var cDir={.x=0.0,.y=0.0} - var cDir1={.x=0.0,.y=0.0} - var cDir2={.x=0.0,.y=0.0} - cDir1=math.vec2.newp(FlockInteraction(mypred1.range,m_vecNodes_fixed[m_nLabel].d1,EPSILON),mypred1.bearing) - cDir2=math.vec2.newp(FlockInteraction(mypred2.range,m_vecNodes_fixed[m_nLabel].d2,EPSILON),mypred2.bearing) - #cDir1=math.vec2.newp((mypred1.range-m_vecNodes_fixed[m_nLabel].d1),mypred1.bearing) - #cDir2=math.vec2.newp((mypred2.range-m_vecNodes_fixed[m_nLabel].d2),mypred2.bearing) - cDir=math.vec2.add(cDir1,cDir2) - - cDir=math.vec2.scale(cDir,100) - m_navigation.x=cDir.x - m_navigation.y=cDir.y - #log(m_nLabel,"error:",mypred1.range-m_vecNodes_fixed[m_nLabel].d1,"and",mypred2.range-m_vecNodes_fixed[m_nLabel].d2) - log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1) -} -#move -uav_moveto(m_navigation.x,m_navigation.y) -} - -function action(){ - statef=action - UAVSTATE="GRAPH" -} - -# -# Executed at init -# -function init() { - # - #Adjust parameters here - # - m_unResponseTimeThreshold=10 - m_unLabelSearchWaitTime=10 - m_fTargetDistanceTolerance=10 - m_unJoiningLostPeriod=100 - - # - # Join Swarm - # - uav_initswarm() - Reset(); -} - -# -# Executed every step -# -function step(){ - uav_rccmd() - uav_neicmd() - #update the graph - UpdateNodeInfo() - #reset message package to be sent - m_selfMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE"} - # - #act according to current state - # - if(UAVSTATE=="GRAPH"){ - if(m_eState=="STATE_FREE") - DoFree() - else if(m_eState=="STATE_ESCAPE") - DoEscape() - else if(m_eState=="STATE_ASKING") - DoAsking() - else if(m_eState=="STATE_JOINING") - DoJoining() - else if(m_eState=="STATE_JOINED") - DoJoined() - else if(m_eState=="STATE_LOCK") - DoLock() - } - - statef() - - - debug(m_eState,m_nLabel) - log("Current state: ", UAVSTATE) - log("Swarm size: ", ROBOTS) - #navigation - - #broadcast messag - neighbors.broadcast("msg",m_selfMessage) - - # - #clean message storage - m_MessageState={}#store received neighbour message - m_MessageLable={}#store received neighbour message - m_MessageReqLable={}#store received neighbour message - m_MessageReqID={}#store received neighbour message - m_MessageSide={}#store received neighbour message - m_MessageResponse={}#store received neighbour message - m_MessageRange={}#store received neighbour message - m_MessageBearing={}#store received neighbour message - m_neighbourCunt=0 - - - #step cunt+1 - step_cunt=step_cunt+1 -} - -# -# Executed when reset -# -function Reset(){ - m_vecNodes={} - m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary - m_vecNodes_fixed={} - m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph") - m_nLabel=-1 - - #start listening - start_listen() - # - #set initial state, only one robot choose [A], while the rest choose [B] - # - #[A]The robot used to triger the formation process is defined as joined, - if(id==0){ - m_nLabel=0 - TransitionToJoined() - } - #[B]Other robots are defined as free. - else{ - TransitionToFree() - } -} - -# -# Executed upon destroy -# -function destroy() { - #clear neighbour message - uav_moveto(0.0,0.0) - m_vecNodes={} - #stop listening - neighbors.ignore("msg") -} diff --git a/buzz_scripts/include/barrier.bzz b/buzz_scripts/include/barrier.bzz index 6192b65..8c55806 100644 --- a/buzz_scripts/include/barrier.bzz +++ b/buzz_scripts/include/barrier.bzz @@ -12,9 +12,9 @@ BARRIER_VSTIG = 11 # # Sets a barrier # -function barrier_set(threshold, transf, resumef) { +function barrier_set(threshold, transf, resumef, bdt) { statef = function() { - barrier_wait(threshold, transf, resumef); + barrier_wait(threshold, transf, resumef, bdt); } barrier = stigmergy.create(BARRIER_VSTIG) } @@ -31,14 +31,16 @@ function barrier_ready() { # BARRIER_TIMEOUT = 200 timeW=0 -function barrier_wait(threshold, transf, resumef) { - barrier.get(id) +function barrier_wait(threshold, transf, resumef, bdt) { + #barrier.get(id) barrier.put(id, 1) UAVSTATE = "BARRIERWAIT" + if(bdt!=-1) + neighbors.broadcast("cmd", brd) if(barrier.size() >= threshold) { # getlowest() transf() - } else if(timeW>=BARRIER_TIMEOUT) { + } else if(timeW >= BARRIER_TIMEOUT) { barrier = nil resumef() timeW=0 diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 10d7118..180c6cc 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -11,8 +11,6 @@ goal = {.range=0, .bearing=0} function uav_initswarm() { s = swarm.create(1) s.join() - statef=turnedoff - UAVSTATE = "TURNEDOFF" } function turnedoff() { @@ -30,7 +28,7 @@ function takeoff() { statef=takeoff if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS,action,land) + barrier_set(ROBOTS,action,land,22) barrier_ready() } else { @@ -49,7 +47,7 @@ function land() { uav_land() } else { - barrier_set(ROBOTS,turnedoff,land) + barrier_set(ROBOTS,turnedoff,land,21) barrier_ready() timeW=0 #barrier = nil @@ -129,23 +127,22 @@ function uav_rccmd() { function uav_neicmd() { neighbors.listen("cmd", - function(vid, value, rid) { - print("Got (", vid, ",", value, ") from robot #", rid) - if(value==22 and UAVSTATE!="TAKEOFF") { - statef=takeoff - } else if(value==21) { - statef=land - } else if(value==400 and UAVSTATE=="IDLE") { - uav_arm() - } else if(value==401 and UAVSTATE=="IDLE"){ - 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(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and UAVSTATE!="TAKEOFF") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and UAVSTATE=="IDLE") { + uav_arm() + } else if(value==401 and UAVSTATE=="IDLE"){ + uav_disarm() + } else if(value==16){ + # neighbors.listen("gt",function(vid, value, rid) { + # print("Got (", vid, ",", value, ") from robot #", rid) + # # if(gt.id == id) statef=goto + # }) + } }) } diff --git a/buzz_scripts/testalone.bzz b/buzz_scripts/testalone.bzz index e607d4e..7c987d6 100644 --- a/buzz_scripts/testalone.bzz +++ b/buzz_scripts/testalone.bzz @@ -12,7 +12,8 @@ function action() { # Executed once at init time. function init() { - uav_initswarm() + statef=turnedoff + UAVSTATE = "TURNEDOFF" uav_initstig() } diff --git a/include/roscontroller.h b/include/roscontroller.h index ae0b7d0..bc5e117 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -111,6 +111,7 @@ private: ros::Subscriber users_sub; ros::Subscriber battery_sub; ros::Subscriber payload_sub; + ros::Subscriber flight_estatus_sub; ros::Subscriber flight_status_sub; ros::Subscriber obstacle_sub; ros::Subscriber Robot_id_sub; diff --git a/launch/launch_config/husky.yaml b/launch/launch_config/husky.yaml deleted file mode 100644 index c878cc4..0000000 --- a/launch/launch_config/husky.yaml +++ /dev/null @@ -1,17 +0,0 @@ -topics: - gps : global_position - localpos : local_position - battery : power_status - status : flight_status - fcclient : dji_mavcmd - setpoint : setpoint_position/local - armclient: dji_mavarm - modeclient: dji_mavmode - altitude: rel_alt - stream: set_stream_rate - -type: - gps : sensor_msgs/NavSatFix - battery : mavros_msgs/BatteryStatus - status : mavros_msgs/ExtendedState - altitude: std_msgs/Float64 diff --git a/launch/launch_config/m100.yaml b/launch/launch_config/m100.yaml deleted file mode 100644 index c878cc4..0000000 --- a/launch/launch_config/m100.yaml +++ /dev/null @@ -1,17 +0,0 @@ -topics: - gps : global_position - localpos : local_position - battery : power_status - status : flight_status - fcclient : dji_mavcmd - setpoint : setpoint_position/local - armclient: dji_mavarm - modeclient: dji_mavmode - altitude: rel_alt - stream: set_stream_rate - -type: - gps : sensor_msgs/NavSatFix - battery : mavros_msgs/BatteryStatus - status : mavros_msgs/ExtendedState - altitude: std_msgs/Float64 diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml deleted file mode 100644 index f9ca5d2..0000000 --- a/launch/launch_config/solo.yaml +++ /dev/null @@ -1,21 +0,0 @@ -topics: - gps : mavros/global_position/global - battery : mavros/battery - status : mavros/state - fcclient: mavros/cmd/command - setpoint: mavros/setpoint_position/local - armclient: mavros/cmd/arming - modeclient: mavros/set_mode - localpos: mavros/local_position/pose - stream: mavros/set_stream_rate - altitude: mavros/global_position/rel_alt -type: - gps : sensor_msgs/NavSatFix - # for SITL Solo - battery : mavros_msgs/BatteryState - # for solo - #battery : mavros_msgs/BatteryStatus - status : mavros_msgs/State - altitude: std_msgs/Float64 -environment : - environment : solo-simulator diff --git a/launch/launch_config/topics.yaml b/launch/launch_config/topics.yaml index 8f1295b..e7358cd 100644 --- a/launch/launch_config/topics.yaml +++ b/launch/launch_config/topics.yaml @@ -2,7 +2,7 @@ topics: gps : global_position/global battery : battery status : state - estatus : extended_state + estatus : extended_state fcclient: cmd/command setpoint: setpoint_position/local armclient: cmd/arming diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch index de7857d..c2531f4 100644 --- a/launch/rosbuzz.launch +++ b/launch/rosbuzz.launch @@ -7,7 +7,7 @@ - + diff --git a/launch/rosbuzzd.launch b/launch/rosbuzzd.launch new file mode 100644 index 0000000..de7857d --- /dev/null +++ b/launch/rosbuzzd.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 8b883e9..3d5531d 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -487,7 +487,7 @@ int create_stig_tables() { if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); - ROS_ERROR("%s: Error loading Buzz script", bo_filename); + ROS_ERROR("[%i] %s: Error loading Buzz script", Robot_id, bo_filename); return 0; } /* Register hook functions */ @@ -550,14 +550,14 @@ int create_stig_tables() { if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); - fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME); + ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update)", Robot_id); return 0; } // Register hook functions if(buzz_register_hooks() != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); - fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME); + ROS_ERROR("[%i] Error registering hooks (update)", Robot_id); return 0; } /* Create vstig tables @@ -606,14 +606,14 @@ int create_stig_tables() { if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); - fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME); + ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update init)", Robot_id); return 0; } // Register hook functions if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); - fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME); + ROS_ERROR("[%i] Error registering hooks (update init)", Robot_id); return 0; } /* Create vstig tables diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index aa04afa..639c9ea 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -337,7 +337,7 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle &node_handle) std::string battery_topic; node_handle.getParam("topics/battery", battery_topic); - m_smTopic_infos.insert(pair(battery_topic, "mavros_msgs/BatteryState")); + m_smTopic_infos.insert(pair(battery_topic, "mavros_msgs/BatteryStatus")); std::string status_topic; node_handle.getParam("topics/status", status_topic); @@ -439,9 +439,9 @@ 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_estatus_sub = n_c.subscribe(it->first, 5, &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, 5, &roscontroller::flight_status_update, this); } else if (it->second == "mavros_msgs/BatteryStatus") { battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this); } else if (it->second == "sensor_msgs/NavSatFix") { @@ -459,31 +459,17 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c) /-------------------------------------------------------*/ std::string roscontroller::Compile_bzz(std::string bzzfile_name) { - /*TODO: change to bzzc instead of bzzparse and also add -I for includes*/ /*Compile the buzz code .bzz to .bo*/ stringstream bzzfile_in_compile; std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - // bzzfile_in_compile << path << "/"; - // path = bzzfile_in_compile.str(); - // bzzfile_in_compile.str(""); std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); name = name.substr(0, name.find_last_of(".")); bzzfile_in_compile << "bzzc -I " << path - << "include/"; //<<" "<