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

fix rviz res

This commit is contained in:
dave 2017-12-20 13:45:22 -05:00
parent 701991adc2
commit 661f7897c3
3 changed files with 11 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
buzz_scripts/include/rrtstar.bzz
View File

@ -27,7 +27,7 @@ function pathPlanner(m_goal, m_pos, kh4) {
mapgoal = getcell(math.vec2.add(m_goal, m_pos)) mapgoal = getcell(math.vec2.add(m_goal, m_pos))
#print_map(map) #print_map(map)
#export_map(map) export_map(map)
# search for a path # search for a path
return RRTSTAR(mapgoal,math.vec2.new(5 * GSCALE.x, 5 * GSCALE.y)) #size(map[1])/20.0,size(map[1])/20.0)) return RRTSTAR(mapgoal,math.vec2.new(5 * GSCALE.x, 5 * GSCALE.y)) #size(map[1])/20.0,size(map[1])/20.0))

6
src/buzzuav_closures.cpp
View File

@ -184,12 +184,12 @@ int buzz_exportmap(buzzvm_t vm)
buzzvm_lload(vm, 1); buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary
buzzobj_t t = buzzvm_stack_at(vm, 1); buzzobj_t t = buzzvm_stack_at(vm, 1);
for(int32_t i = 1; i < buzzdict_size(t->t.value); ++i) { for(int32_t i = 1; i <= buzzdict_size(t->t.value); ++i) {
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushi(vm, i); buzzvm_pushi(vm, i);
buzzvm_tget(vm); buzzvm_tget(vm);
std::map<int, int> row; std::map<int, int> row;
for(int32_t j = 1; j < buzzdict_size(buzzvm_stack_at(vm, 1)->t.value); ++j) { for(int32_t j = 1; j <= buzzdict_size(buzzvm_stack_at(vm, 1)->t.value); ++j) {
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushi(vm, j); buzzvm_pushi(vm, j);
buzzvm_tget(vm); buzzvm_tget(vm);
@ -199,6 +199,8 @@ int buzz_exportmap(buzzvm_t vm)
grid.insert(std::pair<int,std::map<int, int>>(i,row)); grid.insert(std::pair<int,std::map<int, int>>(i,row));
buzzvm_pop(vm); buzzvm_pop(vm);
} }
// DEBUG
// ROS_INFO("----- Recorded a grid of %i(%i)", grid.size(), buzzdict_size(t->t.value));
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }

9
src/roscontroller.cpp
View File

@ -461,12 +461,14 @@ void roscontroller::grid_publisher()
int g_h = grid.size(); int g_h = grid.size();
if(g_w!=0 && g_h!=0) { if(g_w!=0 && g_h!=0) {
// DEBUG
//ROS_INFO("------> Publishing a grid of %i x %i", g_h, g_w);
auto current_time = ros::Time::now(); auto current_time = ros::Time::now();
nav_msgs::OccupancyGrid grid_msg; nav_msgs::OccupancyGrid grid_msg;
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 = gridMap.getResolution(); grid_msg.info.resolution = 0.1;//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;
@ -476,12 +478,13 @@ void roscontroller::grid_publisher()
grid_msg.info.origin.orientation.y = 0.0; grid_msg.info.origin.orientation.y = 0.0;
grid_msg.info.origin.orientation.z = 0.0; grid_msg.info.origin.orientation.z = 0.0;
grid_msg.info.origin.orientation.w = 1.0; grid_msg.info.origin.orientation.w = 1.0;
grid_msg.data.resize(g_w*g_h); grid_msg.data.resize(g_w * g_h);
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) {
grid_msg.data[itr->first*g_w+itc->first] = round(itc->second*100.0); //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] = round(itc->second*100.0);
} }
} }
grid_pub.publish(grid_msg); grid_pub.publish(grid_msg);