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

fix proxmity topic

This commit is contained in:
dave 2017-12-20 16:10:30 -05:00
parent 081269cdab
commit 1f2fae047a
3 changed files with 16 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
buzz_scripts/testRRT.bzz
View File

@ -13,10 +13,10 @@ function action() {
function init() { function init() {
uav_initstig() uav_initstig()
uav_initswarm() uav_initswarm()
#statef=turnedoff statef=turnedoff
#UAVSTATE = "TURNEDOFF" BVMSTATE = "TURNEDOFF"
statef = takeoff #statef = takeoff
BVMSTATE = "TAKEOFF" #BVMSTATE = "TAKEOFF"
} }
# Executed at each time step. # Executed at each time step.
@ -26,6 +26,12 @@ function step() {
statef() statef()
log("Current state: ", BVMSTATE) log("Current state: ", BVMSTATE)
log("Obstacles: ")
reduce(proximity,
function(key, value, acc) {
log(key, " - ", value.angle, value.value)
return acc
}, math.vec2.new(0, 0))
} }
# Executed once when the robot (or the simulator) is reset. # Executed once when the robot (or the simulator) is reset.

1
launch/launch_config/topics.yaml
View File

@ -10,3 +10,4 @@ topics:
localpos: local_position/pose localpos: local_position/pose
stream: set_stream_rate stream: set_stream_rate
altitude: global_position/rel_alt altitude: global_position/rel_alt
obstacles: obstacles

9
src/roscontroller.cpp
View File

@ -318,7 +318,7 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle)
system("rosnode kill rosbuzz_node"); system("rosnode kill rosbuzz_node");
} }
node_handle.getParam("obstacles", obstacles_topic); node_handle.getParam("topics/obstacles", obstacles_topic);
} }
void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c)
@ -468,7 +468,7 @@ void roscontroller::grid_publisher()
grid_msg.header.frame_id = "/world"; grid_msg.header.frame_id = "/world";
grid_msg.header.stamp = current_time; grid_msg.header.stamp = current_time;
grid_msg.info.map_load_time = current_time; // Same as header stamp as we do not load the map. grid_msg.info.map_load_time = current_time; // Same as header stamp as we do not load the map.
grid_msg.info.resolution = 0.1;//gridMap.getResolution(); grid_msg.info.resolution = 0.01;//gridMap.getResolution();
grid_msg.info.width = g_w; grid_msg.info.width = g_w;
grid_msg.info.height = g_h; grid_msg.info.height = g_h;
grid_msg.info.origin.position.x = 0.0; grid_msg.info.origin.position.x = 0.0;
@ -483,8 +483,9 @@ void roscontroller::grid_publisher()
for (itr=grid.begin(); itr!=grid.end(); ++itr) { for (itr=grid.begin(); itr!=grid.end(); ++itr) {
std::map<int,int>::iterator itc = itr->second.begin(); std::map<int,int>::iterator itc = itr->second.begin();
for (itc=itr->second.begin(); itc!=itr->second.end(); ++itc) { for (itc=itr->second.begin(); itc!=itr->second.end(); ++itc) {
//ROS_INFO("--------------> index: %i (%i,%i): %i", (itr->first-1)*g_w+itc->first, itr->first, itc->first, round(itc->second*100.0)); grid_msg.data[(itr->first-1)*g_w+itc->first] = itc->second;
grid_msg.data[(itr->first-1)*g_w+itc->first] = round(itc->second*100.0); // DEBUG
//ROS_INFO("--------------> index: %i (%i,%i): %i", (itr->first-1)*g_w+itc->first, itr->first, itc->first, grid_msg.data[(itr->first-1)*g_w+itc->first]);
} }
} }
grid_pub.publish(grid_msg); grid_pub.publish(grid_msg);