Commit after merge
This commit is contained in:
pyhs
parent
edc1b31174
commit
30fe6e1599
|
|
@ -5,8 +5,6 @@ if(UNIX)
|
|||
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11")
|
||||
endif()
|
||||
|
||||
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/buzz_scripts/graphform.bzz.in ${CMAKE_CURRENT_SOURCE_DIR}/buzz_scripts/graphform.bzz)
|
||||
|
||||
## Find catkin macros and libraries
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
|
|
|
|||
|
|
@ -793,9 +793,9 @@ function step(){
|
|||
#
|
||||
function Reset(){
|
||||
m_vecNodes={}
|
||||
m_vecNodes = parse_graph("${CMAKE_CURRENT_SOURCE_DIR}/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary
|
||||
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("${CMAKE_CURRENT_SOURCE_DIR}/buzz_scripts/include/Graph_fixed.graph")
|
||||
m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph")
|
||||
m_nLabel=-1
|
||||
|
||||
#start listening
|
||||
|
|
@ -1,5 +1,5 @@
|
|||
0 -1 -1 -1
|
||||
1 0 1000.0 0.0
|
||||
2 0 1000.0 1.57
|
||||
3 0 1000.0 3.14
|
||||
4 0 1000.0 4.71
|
||||
0 -1 -1 -1 3000.0
|
||||
1 0 1000.0 0.0 5000.0
|
||||
2 0 1000.0 1.57 7000.0
|
||||
3 0 1000.0 3.14 9000.0
|
||||
4 0 1000.0 4.71 11000.0
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
0 -1 -1 -1 -1
|
||||
1 0 1000.0 -1 -1
|
||||
2 0 1000.0 1 1414.2
|
||||
3 0 1000.0 2 1414.2
|
||||
4 0 1000.0 1 1414.2
|
||||
1 0 10.0 -1 -1
|
||||
2 0 10.0 1 1414.2
|
||||
3 0 10.0 2 1414.2
|
||||
4 0 10.0 1 1414.2
|
||||
|
|
@ -0,0 +1,111 @@
|
|||
#Table of the nodes in the graph
|
||||
m_vecNodes={}
|
||||
m_vecNodes_fixed={}
|
||||
m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
|
||||
.Lable = 0, # Lable of the point
|
||||
.Pred = -1, # Lable of its predecessor
|
||||
.distance = -1, # distance to the predecessor
|
||||
.bearing = -1, # bearing form the predecessor to this dot
|
||||
.height = 3000, # height of this dot
|
||||
.State="UNASSIGNED",
|
||||
.StateAge=0
|
||||
}
|
||||
m_vecNodes[1] = {
|
||||
.Lable = 1,
|
||||
.Pred = 0,
|
||||
.distance = 1000,
|
||||
.bearing = 0.0,
|
||||
.height = 5000,
|
||||
.State="UNASSIGNED",
|
||||
.StateAge=0
|
||||
}
|
||||
m_vecNodes[2] = {
|
||||
.Lable = 2,
|
||||
.Pred = 0,
|
||||
.distance = 1000,
|
||||
.bearing = 1.57,
|
||||
.height = 7000,
|
||||
.State="UNASSIGNED",
|
||||
.StateAge=0
|
||||
}
|
||||
m_vecNodes[3] = {
|
||||
.Lable = 3,
|
||||
.Pred = 0,
|
||||
.distance = 1000,
|
||||
.bearing = 3.14,
|
||||
.height = 9000,
|
||||
.State="UNASSIGNED",
|
||||
.StateAge=0
|
||||
}
|
||||
m_vecNodes[4] = {
|
||||
.Lable = 4,
|
||||
.Pred = 0,
|
||||
.distance = 1000,
|
||||
.bearing = 4.71,
|
||||
.height = 11000,
|
||||
.State="UNASSIGNED",
|
||||
.StateAge=0
|
||||
}
|
||||
|
||||
#
|
||||
# Graph parsing
|
||||
#
|
||||
function parse_graph(fname) {
|
||||
# Graph data
|
||||
var gd = {}
|
||||
# Open the file
|
||||
var fd = io.fopen(fname, "r")
|
||||
if(not fd) {
|
||||
log("Can't open '", fname, "'")
|
||||
return nil
|
||||
}
|
||||
# Parse the file line by line
|
||||
var rrec # Record read from line
|
||||
var grec # Record parsed into graph
|
||||
io.fforeach(fd, function(line) {
|
||||
# Parse file line
|
||||
rrec = string.split(line, "\t ")
|
||||
# Make record
|
||||
gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
|
||||
.Lable = string.toint(rrec[0]), # Lable of the point
|
||||
.Pred = string.toint(rrec[1]), # Lable of its predecessor
|
||||
.distance = string.tofloat(rrec[2]), # distance to the predecessor
|
||||
.bearing = string.tofloat(rrec[3]), # bearing form the predecessor to this dot
|
||||
.height = string.tofloat(rrec[4]), # height of this dot
|
||||
.State="UNASSIGNED",
|
||||
.StateAge=0
|
||||
}})
|
||||
# All done
|
||||
io.fclose(fd)
|
||||
return gd
|
||||
}
|
||||
|
||||
function parse_graph_fixed(fname) {
|
||||
# Graph data
|
||||
var gd = {}
|
||||
# Open the file
|
||||
var fd = io.fopen(fname, "r")
|
||||
if(not fd) {
|
||||
log("Can't open '", fname, "'")
|
||||
return nil
|
||||
}
|
||||
# Parse the file line by line
|
||||
var rrec # Record read from line
|
||||
var grec # Record parsed into graph
|
||||
io.fforeach(fd, function(line) {
|
||||
# Parse file line
|
||||
rrec = string.split(line, "\t ")
|
||||
# Make record
|
||||
gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, pre1, dis2pr1, pre2, ids2pre2
|
||||
.Pred1 = string.toint(rrec[1]), # Pred 1 lable
|
||||
.Pred2 = string.toint(rrec[3]), # Pred 2 lable
|
||||
.d1 = string.tofloat(rrec[2]), # Pred 1 distance
|
||||
.d2 = string.tofloat(rrec[4]), # Pred 2 distance
|
||||
.Lable=string.toint(rrec[0]),
|
||||
.State="UNASSIGNED",
|
||||
.StateAge=0
|
||||
}})
|
||||
# All done
|
||||
io.fclose(fd)
|
||||
return gd
|
||||
}
|
||||
|
|
@ -43,7 +43,7 @@
|
|||
<!-- 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)/script/testflockfev.bzz" />
|
||||
<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"/>
|
||||
|
|
@ -51,7 +51,7 @@
|
|||
<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)/script/stand_by.bzz"/>
|
||||
<param name="stand_by" value="$(find rosbuzz)/buzz_scripts/stand_by.bzz"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
|
|
|
|||
Loading…
Reference in New Issue