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

[<tf...@us...>]

Revision: 16551
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16551&view=rev
Author:   tfoote
Date:     2009-06-01 19:10:08 +0000 (Mon, 01 Jun 2009)

Log Message:
-----------
multirobot nav demo eitan and I brought up

Modified Paths:
--------------
    pkg/trunk/highlevel/test_nav/config/costmap_common_params.yaml
    pkg/trunk/highlevel/test_nav/move_base/base_local_planner_params.yaml
    pkg/trunk/highlevel/test_nav/move_base/move_base.launch
    pkg/trunk/highlevel/test_nav/move_base/navfn_params.yaml
    pkg/trunk/highlevel/test_nav/move_base_local/move_base_local.launch
    pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
    pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
    pkg/trunk/nav/nav_view/src/nav_view/tools.cpp
    pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp

Added Paths:
-----------
    pkg/trunk/highlevel/test_nav/move_base/move_base_multi_robot.launch

Modified: pkg/trunk/highlevel/test_nav/config/costmap_common_params.yaml
===================================================================
--- pkg/trunk/highlevel/test_nav/config/costmap_common_params.yaml	2009-06-01 19:09:42 UTC (rev 16550)
+++ pkg/trunk/highlevel/test_nav/config/costmap_common_params.yaml	2009-06-01 19:10:08 UTC (rev 16551)
@@ -1,13 +1,21 @@
 costmap:
+  #START VOXEL STUFF
+  map_type: voxel
+  origin_z: 0.0
+  z_resolution: 0.2
+  z_voxels: 10
+  unknown_threshold: 10
+  mark_threshold: 0
+  #END VOXEL STUFF
   transform_tolerance: 0.3
   obstacle_range: 2.5
   max_obstacle_height: 2.0
   raytrace_range: 3.0
-  inscribed_radius: 0.325
+#inscribed_radius: 0.325
   circumscribed_radius: 0.46
   inflation_radius: 0.55
-  cost_scaling_factor: 25.0
+  cost_scaling_factor: 10.0
   lethal_cost_threshold: 100
   observation_topics: base_scan
   base_scan: {sensor_frame: frame_from_message, data_type: LaserScan, expected_update_rate: 0.2,
-    observation_persistance: 0.0, marking: true, clearing: true}
+    observation_persistance: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08}

Modified: pkg/trunk/highlevel/test_nav/move_base/base_local_planner_params.yaml
===================================================================
--- pkg/trunk/highlevel/test_nav/move_base/base_local_planner_params.yaml	2009-06-01 19:09:42 UTC (rev 16550)
+++ pkg/trunk/highlevel/test_nav/move_base/base_local_planner_params.yaml	2009-06-01 19:10:08 UTC (rev 16551)
@@ -1,10 +1,11 @@
 base_local_planner:
   #Independent settings for the local costmap
   costmap:
-    global_frame: map
+    inscribed_radius: 0.325
+    global_frame: /map
     robot_base_frame: base_link
     update_frequency: 5.0
-    publish_frequency: 0.0
+    publish_frequency: 2.0
     static_map: false
     rolling_window: true
     width: 6.0
@@ -13,10 +14,10 @@
     origin_x: 0.0
     origin_y: 0.0
   transform_tolerance: 0.3
-  costmap_visualization_rate: 2.0
-  world_model: freespace
+  costmap_visualization_rate: 0.0
+  world_model: costmap
   sim_time: 1.7
-  sim_granularity: 0.05
+  sim_granularity: 0.025
   dwa: true
   vx_samples: 3
   vtheta_samples: 20
@@ -28,8 +29,8 @@
   xy_goal_tolerance: 0.1
   yaw_goal_tolerance: 0.05
   goal_distance_bias: 0.8
-  path_distance_bias: 0.6
-  occdist_scale: 0.05
+  path_distance_bias: 0.8
+  occdist_scale: 0.00
   heading_lookahead: 0.325
   oscillation_reset_dist: 0.05
   acc_limit_th: 3.2

Modified: pkg/trunk/highlevel/test_nav/move_base/move_base.launch
===================================================================
--- pkg/trunk/highlevel/test_nav/move_base/move_base.launch	2009-06-01 19:09:42 UTC (rev 16550)
+++ pkg/trunk/highlevel/test_nav/move_base/move_base.launch	2009-06-01 19:10:08 UTC (rev 16551)
@@ -4,13 +4,8 @@
   <param name="/use_sim_time" value="true"/>
     <node pkg="nav" type="move_base" respawn="false" name="move_base" output="screen">
 
-      <param name="global_frame" value="map" />
-      <param name="robot_base_frame" value="base_link" />
+      <param name="footprint_padding" value="0.02" />
 
-      <param name="inscribed_radius" value="0.325" />
-      <param name="circumscribed_radius" value="0.46" />
-      <param name="inflation_radius" value="0.55" />
-
       <rosparam file="$(find test_nav)/config/costmap_common_params.yaml" command="load" ns="navfn" />
       <rosparam file="$(find test_nav)/config/costmap_common_params.yaml" command="load" ns="base_local_planner" />
       <rosparam file="$(find test_nav)/move_base/navfn_params.yaml" command="load" />
@@ -26,17 +21,21 @@
     <node pkg="stage" type="stageros" name="stageros" args="$(optenv ROS_STAGE_GRAPHICS -g) $(find 2dnav_stage)/worlds/willow-pr2-5cm.world" respawn="false" >
       <param name="base_watchdog_timeout" value="0.2"/>
     </node>
+    <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full-0.05.pgm 0.05" respawn="false" />
+    <node pkg="stage" type="stageros" name="stageros" args="$(optenv ROS_STAGE_GRAPHICS -g) $(find 2dnav_stage)/worlds/willow-pr2-5cm.world" respawn="false" >
+      <param name="base_watchdog_timeout" value="0.2"/>
+    </node>
     -->
-    <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full-0.025.pgm 0.025" respawn="false" />
+    <node pkg="map_server" type="map_server" args="$(find willow_maps)/willow-full-0.025.pgm 0.025" />
     <node pkg="stage" type="stageros" name="stageros" args="$(optenv ROS_STAGE_GRAPHICS -g) $(find 2dnav_stage)/worlds/willow-pr2-2.5cm.world" respawn="false" >
       <param name="base_watchdog_timeout" value="0.2"/>
     </node>
     <node pkg="fake_localization" type="fake_localization" respawn="false" />
     <node pkg="nav_view" type="nav_view" respawn="false">
       <remap from="goal" to="/move_base/activate" />
-      <remap from="raw_obstacles" to="/move_base/base_local_planner/raw_obstacles" />
-      <remap from="inflated_obstacles" to="/move_base/base_local_planner/inflated_obstacles" />
-      <remap from="gui_path" to="/move_base/base_local_planner/global_plan" />
+      <remap from="raw_obstacles" to="/move_base/base_local_planner/costmap/raw_obstacles" />
+      <remap from="inflated_obstacles" to="/move_base/base_local_planner/costmap/inflated_obstacles" />
+      <remap from="gui_path" to="/move_base/navfn/plan" />
       <remap from="local_path" to="/move_base/base_local_planner/local_plan" />
       <remap from="robot_footprint" to="/move_base/base_local_planner/robot_footprint"/>
     </node>

Added: pkg/trunk/highlevel/test_nav/move_base/move_base_multi_robot.launch
===================================================================
--- pkg/trunk/highlevel/test_nav/move_base/move_base_multi_robot.launch	                        (rev 0)
+++ pkg/trunk/highlevel/test_nav/move_base/move_base_multi_robot.launch	2009-06-01 19:10:08 UTC (rev 16551)
@@ -0,0 +1,64 @@
+<launch>
+  <master auto="start"/>
+  <group name="wg">
+   <param name="/use_sim_time" value="true"/>  
+
+    <node pkg="map_server" type="map_server" name="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" >
+    </node>
+
+    <node pkg="stage" type="stageros" name="stageros" args="$(optenv ROS_STAGE_GRAPHICS -g) $(find 2dnav_stage)/worlds/willow-pr2-multi.world" respawn="false">
+      <param name="base_watchdog_timeout" value="0.2"/>
+    </node>
+
+    <!-- BEGIN ROBOT 0 -->
+    <group ns="robot_0">
+      <param name="~tf_prefix" value="robot_0" />
+      <node pkg="nav" type="move_base" respawn="false" name="move_base" output="screen">
+        <rosparam file="$(find test_nav)/config/costmap_common_params.yaml" command="load" ns="navfn" />
+        <rosparam file="$(find test_nav)/config/costmap_common_params.yaml" command="load" ns="base_local_planner" />
+        <rosparam file="$(find test_nav)/move_base/navfn_params.yaml" command="load" />
+        <rosparam file="$(find test_nav)/move_base/base_local_planner_params.yaml" command="load" />
+
+      </node>
+
+      <node pkg="fake_localization" type="fake_localization" name="fake_localization" respawn="false">
+      </node>
+
+      <node pkg="nav_view" type="nav_view" name="nav_view" respawn="false">
+        <remap from="goal" to="move_base/activate" />
+        <remap from="raw_obstacles" to="move_base/base_local_planner/costmap/raw_obstacles" />
+        <remap from="inflated_obstacles" to="move_base/base_local_planner/costmap/inflated_obstacles" />
+        <remap from="gui_path" to="move_base/base_local_planner/global_plan" />
+        <remap from="local_path" to="move_base/base_local_planner/local_plan" />
+        <remap from="robot_footprint" to="move_base/base_local_planner/robot_footprint"/>
+      </node>
+    </group>
+    <!-- END ROBOT 0 -->
+
+    <!-- BEGIN ROBOT 1 -->
+    <group ns="robot_1">
+      <param name="~tf_prefix" value="robot_1" />
+      <node pkg="nav" type="move_base" respawn="false" name="move_base" output="screen">
+        <rosparam file="$(find test_nav)/config/costmap_common_params.yaml" command="load" ns="navfn" />
+        <rosparam file="$(find test_nav)/config/costmap_common_params.yaml" command="load" ns="base_local_planner" />
+        <rosparam file="$(find test_nav)/move_base/navfn_params.yaml" command="load" />
+        <rosparam file="$(find test_nav)/move_base/base_local_planner_params.yaml" command="load" />
+
+      </node>
+
+      <node pkg="fake_localization" type="fake_localization" name="fake_localization" respawn="false">
+      </node>
+
+      <node pkg="nav_view" type="nav_view" name="nav_view" respawn="false">
+        <remap from="goal" to="move_base/activate" />
+        <remap from="raw_obstacles" to="move_base/base_local_planner/costmap/raw_obstacles" />
+        <remap from="inflated_obstacles" to="move_base/base_local_planner/costmap/inflated_obstacles" />
+        <remap from="gui_path" to="move_base/base_local_planner/global_plan" />
+        <remap from="local_path" to="move_base/base_local_planner/local_plan" />
+        <remap from="robot_footprint" to="move_base/base_local_planner/robot_footprint"/>
+      </node>
+    </group>
+    <!-- END ROBOT 1 -->
+
+  </group>
+</launch>

Modified: pkg/trunk/highlevel/test_nav/move_base/navfn_params.yaml
===================================================================
--- pkg/trunk/highlevel/test_nav/move_base/navfn_params.yaml	2009-06-01 19:09:42 UTC (rev 16550)
+++ pkg/trunk/highlevel/test_nav/move_base/navfn_params.yaml	2009-06-01 19:10:08 UTC (rev 16551)
@@ -2,7 +2,8 @@
   transform_tolerance: 0.3
   #Independent settings for the planner's costmap
   costmap: 
-    global_frame: map
+    inscribed_radius: 0.325
+    global_frame: /map
     robot_base_frame: base_link
     update_frequency: 5.0
     publish_frequency: 0.0

Modified: pkg/trunk/highlevel/test_nav/move_base_local/move_base_local.launch
===================================================================
--- pkg/trunk/highlevel/test_nav/move_base_local/move_base_local.launch	2009-06-01 19:09:42 UTC (rev 16550)
+++ pkg/trunk/highlevel/test_nav/move_base_local/move_base_local.launch	2009-06-01 19:10:08 UTC (rev 16551)
@@ -3,9 +3,6 @@
   <group name="wg">
   <param name="/use_sim_time" value="true"/>
     <node pkg="nav" type="move_base_local" respawn="false" name="move_base_local" output="screen">
-      <param name="global_frame" value="map" />
-      <param name="robot_base_frame" value="base_link" />
-
       <rosparam file="$(find test_nav)/config/costmap_common_params.yaml" command="load" ns="base_local_planner" />
       <rosparam file="$(find test_nav)/move_base_local/base_local_planner_params.yaml" command="load" />
 

Modified: pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp	2009-06-01 19:09:42 UTC (rev 16550)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp	2009-06-01 19:10:08 UTC (rev 16551)
@@ -43,7 +43,7 @@
     ros_node_.advertise<visualization_msgs::Polyline>("~navfn/plan", 1);
 
     //read parameters for the planner
-    ros_node_.param("~/navfn/costmap/global_frame", global_frame_, std::string("map"));
+    ros_node_.param("~/navfn/costmap/global_frame", global_frame_, std::string("/map"));
     ros_node_.param("~/navfn/costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
     ros_node_.param("~/navfn/transform_tolerance", transform_tolerance_, 0.2);
     

Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp	2009-06-01 19:09:42 UTC (rev 16550)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp	2009-06-01 19:10:08 UTC (rev 16551)
@@ -64,7 +64,7 @@
     ros_node.advertise<visualization_msgs::Polyline>("~base_local_planner/global_plan", 1);
     ros_node.advertise<visualization_msgs::Polyline>("~base_local_planner/local_plan", 1);
 
-    ros_node.param("~base_local_planner/costmap/global_frame", global_frame_, string("map"));
+    ros_node.param("~base_local_planner/costmap/global_frame", global_frame_, string("/map"));
     ros_node.param("~base_local_planner/costmap/robot_base_frame", robot_base_frame_, string("base_link"));
     ros_node.param("~base_local_planner/transform_tolerance", transform_tolerance_, 0.2);
     ros_node.param("~base_local_planner/update_plan_tolerance", update_plan_tolerance_, 1.0);

Modified: pkg/trunk/nav/nav_view/src/nav_view/tools.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/tools.cpp	2009-06-01 19:09:42 UTC (rev 16550)
+++ pkg/trunk/nav/nav_view/src/nav_view/tools.cpp	2009-06-01 19:10:08 UTC (rev 16551)
@@ -166,7 +166,7 @@
         printf("setting goal: Position(%.3f, %.3f, %.3f), Orientation(%.3f, %.3f, %.3f, %.3f) = Angle: %.3f\n", goal.pose.position.x, goal.pose.position.y, goal.pose.position.z,
             goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w, angle);
         goal.header.stamp = ros::Time::now();
-        goal.header.frame_id = "map";
+        goal.header.frame_id = "/map";
         ros_node_->publish( "goal", goal );
       }
       else

Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp	2009-06-01 19:09:42 UTC (rev 16550)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp	2009-06-01 19:10:08 UTC (rev 16551)
@@ -63,7 +63,7 @@
     ros_node_.param("~" + prefix + "/costmap/observation_topics", topics_string, string(""));
     ROS_INFO("Subscribed to Topics: %s", topics_string.c_str());
 
-    ros_node_.param("~" + prefix + "/costmap/global_frame", global_frame_, string("map"));
+    ros_node_.param("~" + prefix + "/costmap/global_frame", global_frame_, string("/map"));
     ros_node_.param("~" + prefix + "/costmap/robot_base_frame", robot_base_frame_, string("base_link"));
 
     //we need to make sure that the transform between the robot base frame and the global frame is available
@@ -153,7 +153,7 @@
       robot_srvs::StaticMap::Request map_req;
       robot_srvs::StaticMap::Response map_resp;
       ROS_INFO("Requesting the map...\n");
-      while(!ros::service::call("static_map", map_req, map_resp))
+      while(!ros::service::call("/static_map", map_req, map_resp))
       {
         ROS_INFO("Request failed; trying again...\n");
         usleep(1000000);


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