[Personalrobots-commit] SF.net SVN: personalrobots:[23838] pkg/trunk

[<ei...@us...>]

Revision: 23838
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23838&view=rev
Author:   eitanme
Date:     2009-09-04 18:00:21 +0000 (Fri, 04 Sep 2009)

Log Message:
-----------
Putting some reasoning about unknown space back into the navigation stack so that now you just set parameters to decide whether or not to take unkown space into account when running

Modified Paths:
--------------
    pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
    pkg/trunk/highlevel/move_base_stage/config/costmap_common_params.yaml
    pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
    pkg/trunk/sandbox/doors_planner_core/src/move_base_door_action.cpp
    pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.cpp
    pkg/trunk/sandbox/teleop_anti_collision/src/safe_teleop.cpp
    pkg/trunk/stacks/navigation/base_local_planner/src/costmap_model.cpp
    pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp
    pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_ros.cpp
    pkg/trunk/stacks/navigation/move_base/src/move_base.cpp

Modified: pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp	2009-09-04 17:49:40 UTC (rev 23837)
+++ pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp	2009-09-04 18:00:21 UTC (rev 23838)
@@ -257,7 +257,7 @@
 
     //we'll start our costmap up now that we're active
     planner_cost_map_ros_->start();
-    planner_cost_map_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
+    planner_cost_map_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
 
 
     planner_->setDoor(door,getPose2D(global_pose_),door_transformed);//set the goal into the planner

Modified: pkg/trunk/highlevel/move_base_stage/config/costmap_common_params.yaml
===================================================================
--- pkg/trunk/highlevel/move_base_stage/config/costmap_common_params.yaml	2009-09-04 17:49:40 UTC (rev 23837)
+++ pkg/trunk/highlevel/move_base_stage/config/costmap_common_params.yaml	2009-09-04 18:00:21 UTC (rev 23838)
@@ -3,7 +3,7 @@
 origin_z: 0.0
 z_resolution: 0.2
 z_voxels: 10
-unknown_threshold: 10
+unknown_threshold: 9
 mark_threshold: 0
 #END VOXEL STUFF
 transform_tolerance: 0.3

Modified: pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
===================================================================
--- pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp	2009-09-04 17:49:40 UTC (rev 23837)
+++ pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp	2009-09-04 18:00:21 UTC (rev 23838)
@@ -83,8 +83,8 @@
     tc_ = new TrajectoryPlannerROS("TrajectoryPlannerROS", &tf_, controller_costmap_ros_);
 
     //initially clear any unknown space around the robot
-    planner_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
-    controller_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
+    planner_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
+    controller_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
 
     //TODO:spawn planning thread here?
   }

Modified: pkg/trunk/sandbox/doors_planner_core/src/move_base_door_action.cpp
===================================================================
--- pkg/trunk/sandbox/doors_planner_core/src/move_base_door_action.cpp	2009-09-04 17:49:40 UTC (rev 23837)
+++ pkg/trunk/sandbox/doors_planner_core/src/move_base_door_action.cpp	2009-09-04 18:00:21 UTC (rev 23838)
@@ -256,7 +256,7 @@
 
     //we'll start our costmap up now that we're active
     planner_cost_map_ros_->start();
-    planner_cost_map_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
+    planner_cost_map_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
 
 
     planner_->setDoor(door,getPose2D(global_pose_),door_transformed);//set the goal into the planner

Modified: pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.cpp
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.cpp	2009-09-04 17:49:40 UTC (rev 23837)
+++ pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.cpp	2009-09-04 18:00:21 UTC (rev 23838)
@@ -73,7 +73,7 @@
     ros_node_.param("~min_in_place_vel_th", min_in_place_vel_th_, 0.4);
 
     //initialize the copy of the costmap the controller will use
-    costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
+    costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
     costmap_ros_->getCostmapCopy(costmap_);
 
     string odom_topic, base_cmd_topic, joy_listen_topic, world_model_type;

Modified: pkg/trunk/sandbox/teleop_anti_collision/src/safe_teleop.cpp
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/src/safe_teleop.cpp	2009-09-04 17:49:40 UTC (rev 23837)
+++ pkg/trunk/sandbox/teleop_anti_collision/src/safe_teleop.cpp	2009-09-04 18:00:21 UTC (rev 23838)
@@ -108,8 +108,8 @@
 
 
     //initially clear any unknown space around the robot
-    planner_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
-    controller_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
+    planner_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
+    controller_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
 
     //if we shutdown our costmaps when we're deactivated... we'll do that now
     if(shutdown_costmaps_){

Modified: pkg/trunk/stacks/navigation/base_local_planner/src/costmap_model.cpp
===================================================================
--- pkg/trunk/stacks/navigation/base_local_planner/src/costmap_model.cpp	2009-09-04 17:49:40 UTC (rev 23837)
+++ pkg/trunk/stacks/navigation/base_local_planner/src/costmap_model.cpp	2009-09-04 18:00:21 UTC (rev 23838)
@@ -57,8 +57,8 @@
     //for circular robots the circumscribed radius will equal the inscribed radius and we can do a point check
     if(circumscribed_radius == inscribed_radius){
       unsigned char cost = costmap_.getCost(cell_x, cell_y);
-      //if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE || cost == NO_INFORMATION)
-      if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE)
+      //if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE)
+      if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE || cost == NO_INFORMATION)
         return -1.0;
       return 1.0;
     }
@@ -189,8 +189,8 @@
   double CostmapModel::pointCost(int x, int y){
     unsigned char cost = costmap_.getCost(x, y);
     //if the cell is in an obstacle the path is invalid
-    //if(cost == LETHAL_OBSTACLE || cost == NO_INFORMATION){
-    if(cost == LETHAL_OBSTACLE){
+    //if(cost == LETHAL_OBSTACLE){
+    if(cost == LETHAL_OBSTACLE || cost == NO_INFORMATION){
       return -1;
     }
 

Modified: pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp
===================================================================
--- pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp	2009-09-04 17:49:40 UTC (rev 23837)
+++ pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp	2009-09-04 18:00:21 UTC (rev 23838)
@@ -81,8 +81,8 @@
         costmap.mapToWorld(i, j, wx, wy);
         std::pair<double, double> p(wx, wy);
 
-        //if(costmap.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || costmap.getCost(i, j) == costmap_2d::NO_INFORMATION)
-        if(costmap.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE)
+        //if(costmap.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE)
+        if(costmap.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || costmap.getCost(i, j) == costmap_2d::NO_INFORMATION)
           raw_obstacles.push_back(p);
         else if(costmap.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
           inflated_obstacles.push_back(p);

Modified: pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_ros.cpp	2009-09-04 17:49:40 UTC (rev 23837)
+++ pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_ros.cpp	2009-09-04 18:00:21 UTC (rev 23838)
@@ -260,7 +260,7 @@
       ros_node_.param("origin_z", map_origin_z, 0.0);
 
       int unknown_threshold, mark_threshold;
-      ros_node_.param("unknown_threshold", unknown_threshold, 0);
+      ros_node_.param("unknown_threshold", unknown_threshold, z_voxels);
       ros_node_.param("mark_threshold", mark_threshold, 0);
 
       ROS_ASSERT(z_voxels >= 0 && unknown_threshold >= 0 && mark_threshold >= 0);

Modified: pkg/trunk/stacks/navigation/move_base/src/move_base.cpp
===================================================================
--- pkg/trunk/stacks/navigation/move_base/src/move_base.cpp	2009-09-04 17:49:40 UTC (rev 23837)
+++ pkg/trunk/stacks/navigation/move_base/src/move_base.cpp	2009-09-04 18:00:21 UTC (rev 23838)
@@ -110,8 +110,8 @@
     make_plan_srv_ = ros_node_.advertiseService("~make_plan", &MoveBase::planService, this);
 
     //initially clear any unknown space around the robot
-    planner_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
-    controller_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
+    planner_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
+    controller_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
 
     //if we shutdown our costmaps when we're deactivated... we'll do that now
     if(shutdown_costmaps_){


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.