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))
#print_map(map)
#export_map(map)
export_map(map)
# 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))

6
src/buzzuav_closures.cpp
View File

@ -184,12 +184,12 @@ int buzz_exportmap(buzzvm_t vm)
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary
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_pushi(vm, i);
buzzvm_tget(vm);
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_pushi(vm, j);
buzzvm_tget(vm);
@ -199,6 +199,8 @@ int buzz_exportmap(buzzvm_t vm)
grid.insert(std::pair<int,std::map<int, int>>(i,row));
buzzvm_pop(vm);
}
// DEBUG
// ROS_INFO("----- Recorded a grid of %i(%i)", grid.size(), buzzdict_size(t->t.value));
return buzzvm_ret0(vm);
}

7
src/roscontroller.cpp
View File

@ -461,12 +461,14 @@ void roscontroller::grid_publisher()
int g_h = grid.size();
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();
nav_msgs::OccupancyGrid grid_msg;
grid_msg.header.frame_id = "/world";
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.resolution = gridMap.getResolution();
grid_msg.info.resolution = 0.1;//gridMap.getResolution();
grid_msg.info.width = g_w;
grid_msg.info.height = g_h;
grid_msg.info.origin.position.x = 0.0;
@ -481,7 +483,8 @@ void roscontroller::grid_publisher()
for (itr=grid.begin(); itr!=grid.end(); ++itr) {
std::map<int,int>::iterator itc = itr->second.begin();
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);