Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21700] pkg/trunk
|
From: <ei...@us...> - 2009-08-12 20:32:50
|
Revision: 21700
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21700&view=rev
Author: eitanme
Date: 2009-08-12 20:32:42 +0000 (Wed, 12 Aug 2009)
Log Message:
-----------
Working towards removing Polyline for good. Updated all navigation stack related files to work with the new types
Modified Paths:
--------------
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/nav_view/nav_view_move_base.launch
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/nav_view/nav_view_move_base_local.launch
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/move_base.vcg
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/move_base_local.vcg
pkg/trunk/highlevel/move_base_stage/move_base/nav_view.xml
pkg/trunk/highlevel/move_base_stage/move_base_multi_robot.launch
pkg/trunk/nav/wavefront/src/wavefront_node.cpp
pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/costmap_2d_publisher.h
pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp
pkg/trunk/stacks/navigation/navfn/include/navfn/navfn_ros.h
pkg/trunk/stacks/navigation/navfn/src/navfn_ros.cpp
Removed Paths:
-------------
pkg/trunk/stacks/navigation/costmap_2d/src/costmap_test.cpp
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/nav_view/nav_view_move_base.launch
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/nav_view/nav_view_move_base.launch 2009-08-12 20:29:45 UTC (rev 21699)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/nav_view/nav_view_move_base.launch 2009-08-12 20:32:42 UTC (rev 21700)
@@ -2,10 +2,10 @@
<master auto="start"/>
<node pkg="nav_view" type="nav_view" respawn="false">
<remap from="goal" to="/move_base/activate" />
- <remap from="raw_obstacles" to="/move_base/local_costmap/raw_obstacles" />
+ <remap from="obstacles" to="/move_base/local_costmap/obstacles" />
<remap from="inflated_obstacles" to="/move_base/local_costmap/inflated_obstacles" />
- <remap from="gui_path" to="/move_base/NavfnROS/plan" />
- <remap from="local_path" to="/move_base/TrajectoryPlannerROS/local_plan" />
+ <remap from="global_plan" to="/move_base/NavfnROS/plan" />
+ <remap from="local_plan" to="/move_base/TrajectoryPlannerROS/local_plan" />
<remap from="robot_footprint" to="/move_base/TrajectoryPlannerROS/robot_footprint"/>
</node>
</launch>
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/nav_view/nav_view_move_base_local.launch
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/nav_view/nav_view_move_base_local.launch 2009-08-12 20:29:45 UTC (rev 21699)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/nav_view/nav_view_move_base_local.launch 2009-08-12 20:32:42 UTC (rev 21700)
@@ -2,10 +2,10 @@
<master auto="start"/>
<node pkg="nav_view" type="nav_view" respawn="false">
<remap from="goal" to="/move_base_local/activate" />
- <remap from="raw_obstacles" to="/move_base_local/local_costmap/raw_obstacles" />
+ <remap from="obstacles" to="/move_base_local/local_costmap/obstacles" />
<remap from="inflated_obstacles" to="/move_base_local/local_costmap/inflated_obstacles" />
- <remap from="gui_path" to="/move_base_local/NavfnROS/plan" />
- <remap from="local_path" to="/move_base_local/TrajectoryPlannerROS/local_plan" />
+ <remap from="global_plan" to="/move_base_local/NavfnROS/plan" />
+ <remap from="local_plan" to="/move_base_local/TrajectoryPlannerROS/local_plan" />
<remap from="robot_footprint" to="/move_base_local/TrajectoryPlannerROS/robot_footprint"/>
</node>
</launch>
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/move_base.vcg
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/move_base.vcg 2009-08-12 20:29:45 UTC (rev 21699)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/move_base.vcg 2009-08-12 20:32:42 UTC (rev 21700)
@@ -57,15 +57,11 @@
Floor\ Scan.Style=1
Floor\ Scan.Topic=/base_scan_throttled
Global\ Plan.Alpha=1
-Global\ Plan.ColorR=0.1
+Global\ Plan.ColorR=0
Global\ Plan.ColorG=1
Global\ Plan.ColorB=0
Global\ Plan.Enabled=1
-Global\ Plan.Loop=0
-Global\ Plan.Override\ Color=0
-Global\ Plan.Render\ Operation=0
Global\ Plan.Topic=/move_base/TrajectoryPlannerROS/global_plan
-Global\ Plan.Z\ Position=0
Grid.Alpha=0.5
Grid.Cell\ Size=1
Grid.ColorR=0.5
@@ -98,25 +94,17 @@
Head\ Scan.Style=1
Head\ Scan.Topic=/tilt_scan
Inflated\ Obstacles.Alpha=1
-Inflated\ Obstacles.ColorR=0.1
-Inflated\ Obstacles.ColorG=1
-Inflated\ Obstacles.ColorB=0
+Inflated\ Obstacles.ColorR=0
+Inflated\ Obstacles.ColorG=0
+Inflated\ Obstacles.ColorB=1
Inflated\ Obstacles.Enabled=1
-Inflated\ Obstacles.Loop=0
-Inflated\ Obstacles.Override\ Color=0
-Inflated\ Obstacles.Render\ Operation=1
Inflated\ Obstacles.Topic=/move_base/local_costmap/inflated_obstacles
-Inflated\ Obstacles.Z\ Position=0
Local\ Plan.Alpha=1
-Local\ Plan.ColorR=0.1
-Local\ Plan.ColorG=1
-Local\ Plan.ColorB=0
+Local\ Plan.ColorR=0
+Local\ Plan.ColorG=0
+Local\ Plan.ColorB=1
Local\ Plan.Enabled=1
-Local\ Plan.Loop=0
-Local\ Plan.Override\ Color=0
-Local\ Plan.Render\ Operation=0
Local\ Plan.Topic=/move_base/TrajectoryPlannerROS/local_plan
-Local\ Plan.Z\ Position=0
Map.Alpha=0.7
Map.Enabled=1
Map.Request\ Frequency=0
@@ -138,39 +126,27 @@
New\ Ground\ Plane\ Cloud.Selectable=1
New\ Ground\ Plane\ Cloud.Style=1
New\ Ground\ Plane\ Cloud.Topic=/ground_object_cloud
+Obstacles.Alpha=1
+Obstacles.ColorR=1
+Obstacles.ColorG=0
+Obstacles.ColorB=0
+Obstacles.Enabled=1
+Obstacles.Topic=/move_base/local_costmap/obstacles
Origin\ Axes.Enabled=0
Origin\ Axes.Length=1
Origin\ Axes.Radius=0.1
Planner\ Plan.Alpha=1
-Planner\ Plan.ColorR=1
-Planner\ Plan.ColorG=0.572549
+Planner\ Plan.ColorR=0
+Planner\ Plan.ColorG=1
Planner\ Plan.ColorB=0
Planner\ Plan.Enabled=1
-Planner\ Plan.Loop=0
-Planner\ Plan.Override\ Color=1
-Planner\ Plan.Render\ Operation=0
Planner\ Plan.Topic=/move_base/NavfnROS/plan
-Planner\ Plan.Z\ Position=0
-Raw\ Obstacles.Alpha=1
-Raw\ Obstacles.ColorR=0.1
-Raw\ Obstacles.ColorG=1
-Raw\ Obstacles.ColorB=0
-Raw\ Obstacles.Enabled=1
-Raw\ Obstacles.Loop=0
-Raw\ Obstacles.Override\ Color=0
-Raw\ Obstacles.Render\ Operation=1
-Raw\ Obstacles.Topic=/move_base/local_costmap/raw_obstacles
-Raw\ Obstacles.Z\ Position=0
Robot\ Footprint.Alpha=1
-Robot\ Footprint.ColorR=0.1
-Robot\ Footprint.ColorG=1
+Robot\ Footprint.ColorR=1
+Robot\ Footprint.ColorG=0
Robot\ Footprint.ColorB=0
Robot\ Footprint.Enabled=1
-Robot\ Footprint.Loop=0
-Robot\ Footprint.Override\ Color=0
-Robot\ Footprint.Render\ Operation=0
Robot\ Footprint.Topic=/move_base/TrajectoryPlannerROS/robot_footprint
-Robot\ Footprint.Z\ Position=0
Robot\ Model.Alpha=0.5
Robot\ Model.Collision\ Enabled=0
Robot\ Model.Enabled=1
@@ -178,118 +154,6 @@
Robot\ Model.Robot\ Description=robot_description
Robot\ Model.Update\ Rate=0.1
Robot\ Model.Visual\ Enabled=1
-Robot\:\ Robot\ Model\ Link\ base_laserShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ base_laserShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ base_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ base_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ bl_caster_l_wheel_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ bl_caster_l_wheel_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ bl_caster_r_wheel_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ bl_caster_r_wheel_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ bl_caster_rotation_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ bl_caster_rotation_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ br_caster_l_wheel_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ br_caster_l_wheel_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ br_caster_r_wheel_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ br_caster_r_wheel_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ br_caster_rotation_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ br_caster_rotation_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ fl_caster_l_wheel_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ fl_caster_l_wheel_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ fl_caster_r_wheel_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ fl_caster_r_wheel_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ fl_caster_rotation_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ fl_caster_rotation_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ fr_caster_l_wheel_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ fr_caster_l_wheel_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ fr_caster_r_wheel_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ fr_caster_r_wheel_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ fr_caster_rotation_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ fr_caster_rotation_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ head_pan_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ head_pan_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ head_plate_frameShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ head_plate_frameShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ head_tilt_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ head_tilt_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ high_def_frameShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ high_def_frameShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ high_def_optical_frameShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ high_def_optical_frameShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_elbow_flex_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_elbow_flex_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_forearm_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_forearm_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_forearm_roll_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_forearm_roll_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_l_finger_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_l_finger_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_l_finger_tip_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_l_finger_tip_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_palm_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_palm_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_r_finger_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_r_finger_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_r_finger_tip_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_r_finger_tip_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_tool_frameShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_tool_frameShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_shoulder_lift_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_shoulder_lift_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_shoulder_pan_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_shoulder_pan_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_upper_arm_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_upper_arm_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_upper_arm_roll_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_upper_arm_roll_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_wrist_flex_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_wrist_flex_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_wrist_roll_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_wrist_roll_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ laser_tilt_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ laser_tilt_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ laser_tilt_mount_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ laser_tilt_mount_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_elbow_flex_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_elbow_flex_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_forearm_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_forearm_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_forearm_roll_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_forearm_roll_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_l_finger_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_l_finger_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_l_finger_tip_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_l_finger_tip_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_palm_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_palm_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_r_finger_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_r_finger_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_r_finger_tip_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_r_finger_tip_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_tool_frameShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_tool_frameShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_shoulder_lift_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_shoulder_lift_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_shoulder_pan_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_shoulder_pan_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_upper_arm_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_upper_arm_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_upper_arm_roll_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_upper_arm_roll_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_wrist_flex_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_wrist_flex_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_wrist_roll_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_wrist_roll_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ stereo_l_stereo_camera_frameShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ stereo_l_stereo_camera_frameShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ stereo_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ stereo_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ stereo_optical_frameShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ stereo_optical_frameShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ stereo_r_stereo_camera_frameShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ stereo_r_stereo_camera_frameShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ torso_lift_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ torso_lift_linkShow\ Trail=0
Stereo\ Cloud.Alpha=1
Stereo\ Cloud.Autocompute\ Intensity\ Bounds=1
Stereo\ Cloud.Billboard\ Size=0.01
@@ -315,8 +179,8 @@
Transforms.Update\ Rate=1
Visualization\ Markers.Enabled=1
Camera\ Type=Orbit
-Camera\ Config=0.510797 1.5408 17.9393 -25.91 2.90947 -25.9677
-Property\ Grid\ State=selection=FilteredTiltCloud.Enabled;expanded=.Global Options,Visualization Markers,Robot Model,Map,Head Scan,Floor Scan,Transforms.Frames,Transforms.Tree,FilteredTiltCloud,New Ground Plane Cloud,AMCL Cloud,Raw Obstacles,Inflated Obstacles,Robot Footprint,Local Plan,Global Plan,Planner Plan;scrollpos=0,88;splitterpos=248,483;ispageselected=1
+Camera\ Config=0.001 1.5758 20.1811 -29.0894 -3.0576 -20.589
+Property\ Grid\ State=selection=Planner Plan.Color;expanded=.Global Options,Visualization Markers,Robot Model,Map,Head Scan,Floor Scan,Transforms.Frames,Transforms.Tree,FilteredTiltCloud,New Ground Plane Cloud,AMCL Cloud,Obstacles,Inflated Obstacles,Robot Footprint,Global Plan,Local Plan,Planner Plan;scrollpos=0,64;splitterpos=249,485;ispageselected=1
[Display0]
Type=Grid
Name=Grid
@@ -360,20 +224,20 @@
Type=ParticleCloud
Name=AMCL Cloud
[Display14]
-Type=PolyLine
-Name=Raw Obstacles
+Type=GridCells
+Name=Obstacles
[Display15]
-Type=PolyLine
+Type=GridCells
Name=Inflated Obstacles
[Display16]
-Type=PolyLine
+Type=Polygon
Name=Robot Footprint
[Display17]
-Type=PolyLine
-Name=Local Plan
-[Display18]
-Type=PolyLine
+Type=Path
Name=Global Plan
+[Display18]
+Type=Path
+Name=Local Plan
[Display19]
-Type=PolyLine
+Type=Path
Name=Planner Plan
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/move_base_local.vcg
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/move_base_local.vcg 2009-08-12 20:29:45 UTC (rev 21699)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/move_base_local.vcg 2009-08-12 20:32:42 UTC (rev 21700)
@@ -1,8 +1,8 @@
Background\ ColorR=0
Background\ ColorG=0
Background\ ColorB=0
-Fixed\ Frame=/odom_combined
-Target\ Frame=/odom_combined
+Fixed\ Frame=/map
+Target\ Frame=/map
2D\ Pose\:\ Localized.Angle\ Tolerance=0.1
2D\ Pose\:\ Localized.ColorR=0
2D\ Pose\:\ Localized.ColorG=0.1
@@ -31,11 +31,11 @@
FilteredTiltCloud.Max\ ColorR=0.980392
FilteredTiltCloud.Max\ ColorG=0.721569
FilteredTiltCloud.Max\ ColorB=0.235294
-FilteredTiltCloud.Max\ Intensity=2533
+FilteredTiltCloud.Max\ Intensity=2000.01
FilteredTiltCloud.Min\ ColorR=0.988235
FilteredTiltCloud.Min\ ColorG=0.643137
FilteredTiltCloud.Min\ ColorB=0.156863
-FilteredTiltCloud.Min\ Intensity=0
+FilteredTiltCloud.Min\ Intensity=1999.99
FilteredTiltCloud.Selectable=1
FilteredTiltCloud.Style=1
FilteredTiltCloud.Topic=/tilt_scan_filtered
@@ -57,15 +57,11 @@
Floor\ Scan.Style=1
Floor\ Scan.Topic=/base_scan_throttled
Global\ Plan.Alpha=1
-Global\ Plan.ColorR=0.1
+Global\ Plan.ColorR=0
Global\ Plan.ColorG=1
Global\ Plan.ColorB=0
Global\ Plan.Enabled=1
-Global\ Plan.Loop=0
-Global\ Plan.Override\ Color=0
-Global\ Plan.Render\ Operation=0
Global\ Plan.Topic=/move_base_local/TrajectoryPlannerROS/global_plan
-Global\ Plan.Z\ Position=0
Grid.Alpha=0.5
Grid.Cell\ Size=1
Grid.ColorR=0.5
@@ -85,38 +81,30 @@
Head\ Scan.Billboard\ Size=0.01
Head\ Scan.Channel=0
Head\ Scan.Decay\ Time=2
-Head\ Scan.Enabled=1
+Head\ Scan.Enabled=0
Head\ Scan.Max\ ColorR=1
Head\ Scan.Max\ ColorG=0
Head\ Scan.Max\ ColorB=0.901961
-Head\ Scan.Max\ Intensity=2000.01
+Head\ Scan.Max\ Intensity=4096
Head\ Scan.Min\ ColorR=0.972549
Head\ Scan.Min\ ColorG=0
Head\ Scan.Min\ ColorB=0.870588
-Head\ Scan.Min\ Intensity=101
+Head\ Scan.Min\ Intensity=115
Head\ Scan.Selectable=1
Head\ Scan.Style=1
Head\ Scan.Topic=/tilt_scan
Inflated\ Obstacles.Alpha=1
-Inflated\ Obstacles.ColorR=0.1
-Inflated\ Obstacles.ColorG=1
-Inflated\ Obstacles.ColorB=0
+Inflated\ Obstacles.ColorR=0
+Inflated\ Obstacles.ColorG=0
+Inflated\ Obstacles.ColorB=1
Inflated\ Obstacles.Enabled=1
-Inflated\ Obstacles.Loop=0
-Inflated\ Obstacles.Override\ Color=0
-Inflated\ Obstacles.Render\ Operation=1
Inflated\ Obstacles.Topic=/move_base_local/local_costmap/inflated_obstacles
-Inflated\ Obstacles.Z\ Position=0
Local\ Plan.Alpha=1
-Local\ Plan.ColorR=0.1
-Local\ Plan.ColorG=1
-Local\ Plan.ColorB=0
+Local\ Plan.ColorR=0
+Local\ Plan.ColorG=0
+Local\ Plan.ColorB=1
Local\ Plan.Enabled=1
-Local\ Plan.Loop=0
-Local\ Plan.Override\ Color=0
-Local\ Plan.Render\ Operation=0
Local\ Plan.Topic=/move_base_local/TrajectoryPlannerROS/local_plan
-Local\ Plan.Z\ Position=0
Map.Alpha=0.7
Map.Enabled=1
Map.Request\ Frequency=0
@@ -126,170 +114,46 @@
New\ Ground\ Plane\ Cloud.Billboard\ Size=0.05
New\ Ground\ Plane\ Cloud.Channel=0
New\ Ground\ Plane\ Cloud.Decay\ Time=3
-New\ Ground\ Plane\ Cloud.Enabled=1
+New\ Ground\ Plane\ Cloud.Enabled=0
New\ Ground\ Plane\ Cloud.Max\ ColorR=0.0313726
New\ Ground\ Plane\ Cloud.Max\ ColorG=0.952941
New\ Ground\ Plane\ Cloud.Max\ ColorB=1
-New\ Ground\ Plane\ Cloud.Max\ Intensity=-999999
+New\ Ground\ Plane\ Cloud.Max\ Intensity=1784
New\ Ground\ Plane\ Cloud.Min\ ColorR=0
New\ Ground\ Plane\ Cloud.Min\ ColorG=0
New\ Ground\ Plane\ Cloud.Min\ ColorB=0
-New\ Ground\ Plane\ Cloud.Min\ Intensity=-999999
+New\ Ground\ Plane\ Cloud.Min\ Intensity=1539
New\ Ground\ Plane\ Cloud.Selectable=1
New\ Ground\ Plane\ Cloud.Style=1
New\ Ground\ Plane\ Cloud.Topic=/ground_object_cloud
+Obstacles.Alpha=1
+Obstacles.ColorR=1
+Obstacles.ColorG=0
+Obstacles.ColorB=0
+Obstacles.Enabled=1
+Obstacles.Topic=/move_base_local/local_costmap/obstacles
Origin\ Axes.Enabled=0
Origin\ Axes.Length=1
Origin\ Axes.Radius=0.1
Planner\ Plan.Alpha=1
-Planner\ Plan.ColorR=0.1
+Planner\ Plan.ColorR=0
Planner\ Plan.ColorG=1
Planner\ Plan.ColorB=0
-Planner\ Plan.Enabled=0
-Planner\ Plan.Loop=0
-Planner\ Plan.Override\ Color=0
-Planner\ Plan.Render\ Operation=0
+Planner\ Plan.Enabled=1
Planner\ Plan.Topic=/move_base_local/NavfnROS/plan
-Planner\ Plan.Z\ Position=0
-Raw\ Obstacles.Alpha=1
-Raw\ Obstacles.ColorR=0.1
-Raw\ Obstacles.ColorG=1
-Raw\ Obstacles.ColorB=0
-Raw\ Obstacles.Enabled=1
-Raw\ Obstacles.Loop=0
-Raw\ Obstacles.Override\ Color=0
-Raw\ Obstacles.Render\ Operation=1
-Raw\ Obstacles.Topic=/move_base_local/local_costmap/raw_obstacles
-Raw\ Obstacles.Z\ Position=0
Robot\ Footprint.Alpha=1
-Robot\ Footprint.ColorR=0.1
-Robot\ Footprint.ColorG=1
+Robot\ Footprint.ColorR=1
+Robot\ Footprint.ColorG=0
Robot\ Footprint.ColorB=0
Robot\ Footprint.Enabled=1
-Robot\ Footprint.Loop=0
-Robot\ Footprint.Override\ Color=0
-Robot\ Footprint.Render\ Operation=0
Robot\ Footprint.Topic=/move_base_local/TrajectoryPlannerROS/robot_footprint
-Robot\ Footprint.Z\ Position=0
-Robot\ Model.Alpha=1
+Robot\ Model.Alpha=0.5
Robot\ Model.Collision\ Enabled=0
Robot\ Model.Enabled=1
-Robot\ Model.Mechanism\ Topic=mechanism_state
+Robot\ Model.Mechanism\ Topic=
Robot\ Model.Robot\ Description=robot_description
Robot\ Model.Update\ Rate=0.1
Robot\ Model.Visual\ Enabled=1
-Robot\:\ Robot\ Model\ Link\ base_laserShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ base_laserShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ base_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ base_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ bl_caster_l_wheel_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ bl_caster_l_wheel_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ bl_caster_r_wheel_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ bl_caster_r_wheel_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ bl_caster_rotation_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ bl_caster_rotation_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ br_caster_l_wheel_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ br_caster_l_wheel_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ br_caster_r_wheel_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ br_caster_r_wheel_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ br_caster_rotation_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ br_caster_rotation_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ fl_caster_l_wheel_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ fl_caster_l_wheel_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ fl_caster_r_wheel_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ fl_caster_r_wheel_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ fl_caster_rotation_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ fl_caster_rotation_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ fr_caster_l_wheel_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ fr_caster_l_wheel_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ fr_caster_r_wheel_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ fr_caster_r_wheel_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ fr_caster_rotation_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ fr_caster_rotation_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ head_pan_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ head_pan_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ head_plate_frameShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ head_plate_frameShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ head_tilt_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ head_tilt_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ high_def_frameShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ high_def_frameShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ high_def_optical_frameShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ high_def_optical_frameShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_elbow_flex_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_elbow_flex_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_forearm_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_forearm_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_forearm_roll_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_forearm_roll_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_l_finger_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_l_finger_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_l_finger_tip_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_l_finger_tip_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_palm_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_palm_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_r_finger_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_r_finger_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_r_finger_tip_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_r_finger_tip_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_tool_frameShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_gripper_tool_frameShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_shoulder_lift_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_shoulder_lift_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_shoulder_pan_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_shoulder_pan_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_upper_arm_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_upper_arm_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_upper_arm_roll_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_upper_arm_roll_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_wrist_flex_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_wrist_flex_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ l_wrist_roll_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ l_wrist_roll_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ laser_tilt_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ laser_tilt_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ laser_tilt_mount_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ laser_tilt_mount_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_elbow_flex_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_elbow_flex_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_forearm_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_forearm_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_forearm_roll_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_forearm_roll_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_l_finger_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_l_finger_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_l_finger_tip_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_l_finger_tip_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_palm_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_palm_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_r_finger_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_r_finger_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_r_finger_tip_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_r_finger_tip_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_tool_frameShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_gripper_tool_frameShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_shoulder_lift_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_shoulder_lift_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_shoulder_pan_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_shoulder_pan_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_upper_arm_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_upper_arm_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_upper_arm_roll_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_upper_arm_roll_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_wrist_flex_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_wrist_flex_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ r_wrist_roll_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ r_wrist_roll_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ stereo_l_stereo_camera_frameShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ stereo_l_stereo_camera_frameShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ stereo_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ stereo_linkShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ stereo_optical_frameShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ stereo_optical_frameShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ stereo_r_stereo_camera_frameShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ stereo_r_stereo_camera_frameShow\ Trail=0
-Robot\:\ Robot\ Model\ Link\ torso_lift_linkShow\ Axes=0
-Robot\:\ Robot\ Model\ Link\ torso_lift_linkShow\ Trail=0
Stereo\ Cloud.Alpha=1
Stereo\ Cloud.Autocompute\ Intensity\ Bounds=1
Stereo\ Cloud.Billboard\ Size=0.01
@@ -315,8 +179,8 @@
Transforms.Update\ Rate=1
Visualization\ Markers.Enabled=1
Camera\ Type=Orbit
-Camera\ Config=0.936 1.7208 12.0025 0 0 0
-Property\ Grid\ State=selection=Visualization Markers.Enabled;expanded=.Global Options,Visualization Markers,Robot Model,Map,Head Scan,Floor Scan,Transforms.Frames,Transforms.Tree,FilteredTiltCloud,New Ground Plane Cloud,AMCL Cloud,Raw Obstacles,Inflated Obstacles,Robot Footprint,Local Plan,Global Plan,Planner Plan;scrollpos=0,0;splitterpos=248,483;ispageselected=1
+Camera\ Config=0.001 1.5758 20.1811 -29.0894 -3.0576 -20.589
+Property\ Grid\ State=selection=Local Plan.Alpha;expanded=.Global Options,Visualization Markers,Robot Model,Map,Head Scan,Floor Scan,Transforms.Frames,Transforms.Tree,FilteredTiltCloud,New Ground Plane Cloud,AMCL Cloud,Obstacles,Inflated Obstacles,Robot Footprint,Global Plan,Local Plan,Planner Plan;scrollpos=0,64;splitterpos=249,485;ispageselected=1
[Display0]
Type=Grid
Name=Grid
@@ -360,20 +224,20 @@
Type=ParticleCloud
Name=AMCL Cloud
[Display14]
-Type=PolyLine
-Name=Raw Obstacles
+Type=GridCells
+Name=Obstacles
[Display15]
-Type=PolyLine
+Type=GridCells
Name=Inflated Obstacles
[Display16]
-Type=PolyLine
+Type=Polygon
Name=Robot Footprint
[Display17]
-Type=PolyLine
-Name=Local Plan
-[Display18]
-Type=PolyLine
+Type=Path
Name=Global Plan
+[Display18]
+Type=Path
+Name=Local Plan
[Display19]
-Type=PolyLine
+Type=Path
Name=Planner Plan
Modified: pkg/trunk/highlevel/move_base_stage/move_base/nav_view.xml
===================================================================
--- pkg/trunk/highlevel/move_base_stage/move_base/nav_view.xml 2009-08-12 20:29:45 UTC (rev 21699)
+++ pkg/trunk/highlevel/move_base_stage/move_base/nav_view.xml 2009-08-12 20:32:42 UTC (rev 21700)
@@ -1,10 +1,10 @@
<launch>
<node pkg="nav_view" type="nav_view" respawn="false">
<remap from="goal" to="/move_base/activate" />
- <remap from="obstacles" to="/move_base/local_costmap/obstacles_new" />
- <remap from="inflated_obstacles" to="/move_base/local_costmap/inflated_obstacles_new" />
- <remap from="global_plan" to="/move_base/NavfnROS/plan_new" />
- <remap from="local_plan" to="/move_base/TrajectoryPlannerROS/local_plan_new" />
- <remap from="robot_footprint" to="/move_base/TrajectoryPlannerROS/robot_footprint_new"/>
+ <remap from="obstacles" to="/move_base/local_costmap/obstacles" />
+ <remap from="inflated_obstacles" to="/move_base/local_costmap/inflated_obstacles" />
+ <remap from="global_plan" to="/move_base/NavfnROS/plan" />
+ <remap from="local_plan" to="/move_base/TrajectoryPlannerROS/local_plan" />
+ <remap from="robot_footprint" to="/move_base/TrajectoryPlannerROS/robot_footprint"/>
</node>
</launch>
Modified: pkg/trunk/highlevel/move_base_stage/move_base_multi_robot.launch
===================================================================
--- pkg/trunk/highlevel/move_base_stage/move_base_multi_robot.launch 2009-08-12 20:29:45 UTC (rev 21699)
+++ pkg/trunk/highlevel/move_base_stage/move_base_multi_robot.launch 2009-08-12 20:32:42 UTC (rev 21700)
@@ -26,10 +26,10 @@
<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/local_costmap/raw_obstacles" />
+ <remap from="obstacles" to="move_base/local_costmap/obstacles" />
<remap from="inflated_obstacles" to="move_base/local_costmap/inflated_obstacles" />
- <remap from="gui_path" to="move_base/NavfnROS/plan" />
- <remap from="local_path" to="move_base/TrajectoryPlannerROS/local_plan" />
+ <remap from="global_plan" to="move_base/NavfnROS/plan" />
+ <remap from="local_plan" to="move_base/TrajectoryPlannerROS/local_plan" />
<remap from="robot_footprint" to="move_base/TrajectoryPlannerROS/robot_footprint"/>
</node>
</group>
@@ -52,10 +52,10 @@
<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/local_costmap/raw_obstacles" />
+ <remap from="obstacles" to="move_base/local_costmap/obstacles" />
<remap from="inflated_obstacles" to="move_base/local_costmap/inflated_obstacles" />
- <remap from="gui_path" to="move_base/NavfnROS/plan" />
- <remap from="local_path" to="move_base/TrajectoryPlannerROS/local_plan" />
+ <remap from="global_plan" to="move_base/NavfnROS/plan" />
+ <remap from="local_plan" to="move_base/TrajectoryPlannerROS/local_plan" />
<remap from="robot_footprint" to="move_base/TrajectoryPlannerROS/robot_footprint"/>
</node>
</group>
Modified: pkg/trunk/nav/wavefront/src/wavefront_node.cpp
===================================================================
--- pkg/trunk/nav/wavefront/src/wavefront_node.cpp 2009-08-12 20:29:45 UTC (rev 21699)
+++ pkg/trunk/nav/wavefront/src/wavefront_node.cpp 2009-08-12 20:32:42 UTC (rev 21700)
@@ -68,7 +68,7 @@
Publishes to (name / type):
- @b "cmd_vel" geometry_msgs/Twist : velocity commands to robot
- @b "state" nav_robot_actions/MoveBaseState : current planner state (e.g., goal reached, no path)
-- @b "gui_path" visualization_msgs/Polyline : current global path (for visualization)
+- @b "global_plan" nav_msgs/Path : current global path (for visualization)
<hr>
@@ -108,7 +108,6 @@
#include <nav_msgs/GetMap.h>
// For GUI debug
-#include <visualization_msgs/Polyline.h>
#include <nav_msgs/Path.h>
// For transform support
@@ -195,7 +194,6 @@
// incoming/outgoing messages
geometry_msgs::PoseStamped goalMsg;
//Odometry odomMsg;
- visualization_msgs::Polyline polylineMsg;
nav_msgs::Path pathMsg;
nav_robot_actions::MoveBaseState pstate;
//Odometry prevOdom;
@@ -361,7 +359,6 @@
this->firstodom = true;
advertise<nav_robot_actions::MoveBaseState>("state",1);
- advertise<visualization_msgs::Polyline>("gui_path",1);
advertise<nav_msgs::Path>("global_plan",1);
advertise<geometry_msgs::Twist>("cmd_vel",1);
subscribe("goal", goalMsg, &WavefrontNode::goalReceived,1);
@@ -705,32 +702,17 @@
ros::Time now = ros::Time::now();
if((now - gui_path_last_publish_time) >= gui_publish_rate)
{
- //@todo TODO: Remove polyline completely
- this->polylineMsg.header.frame_id = "map";
- this->polylineMsg.set_points_size(this->plan->path_count);
- this->polylineMsg.color.r = 0;
- this->polylineMsg.color.g = 1.0;
- this->polylineMsg.color.b = 0;
- this->polylineMsg.color.a = 0;
-
this->pathMsg.header.frame_id = "map";
this->pathMsg.set_poses_size(this->plan->path_count);
for(int i=0;i<this->plan->path_count;i++)
{
- this->polylineMsg.points[i].x =
- PLAN_WXGX(this->plan,this->plan->path[i]->ci);
- this->polylineMsg.points[i].y =
- PLAN_WYGY(this->plan,this->plan->path[i]->cj);
- this->polylineMsg.points[i].z = 0;
-
this->pathMsg.poses[i].pose.position.x =
PLAN_WXGX(this->plan,this->plan->path[i]->ci);
this->pathMsg.poses[i].pose.position.y =
PLAN_WYGY(this->plan,this->plan->path[i]->cj);
this->pathMsg.poses[i].pose.position.z = 0;
}
- publish("gui_path", polylineMsg);
publish("global_plan", pathMsg);
gui_path_last_publish_time = now;
Modified: pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
===================================================================
--- pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-08-12 20:29:45 UTC (rev 21699)
+++ pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-08-12 20:32:42 UTC (rev 21700)
@@ -205,7 +205,7 @@
/**
* @brief Publish a plan for visualization purposes
*/
- void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, const ros::Publisher& pub, const ros::Publisher& new_pub, double r, double g, double b, double a);
+ void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, const ros::Publisher& pub, double r, double g, double b, double a);
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);
@@ -224,7 +224,6 @@
std::vector<geometry_msgs::PoseStamped> global_plan_;
bool prune_plan_;
ros::Publisher footprint_pub_, g_plan_pub_, l_plan_pub_;
- ros::Publisher new_footprint_pub_, new_g_plan_pub_, new_l_plan_pub_;
ros::Subscriber odom_sub_;
boost::recursive_mutex odom_lock_;
bool initialized_;
Modified: pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp
===================================================================
--- pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp 2009-08-12 20:29:45 UTC (rev 21699)
+++ pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp 2009-08-12 20:32:42 UTC (rev 21700)
@@ -40,7 +40,6 @@
#include <sys/time.h>
#include <pluginlib/plugin_macros.h>
-#include "visualization_msgs/Polyline.h"
#include "geometry_msgs/PolygonStamped.h"
#include "nav_msgs/Path.h"
@@ -83,16 +82,10 @@
ros::NodeHandle ros_node("~/" + name);
- //adverstise the fact that we'll publish the robot footprint
- footprint_pub_ = ros_node.advertise<visualization_msgs::Polyline>("robot_footprint", 1);
- g_plan_pub_ = ros_node.advertise<visualization_msgs::Polyline>("global_plan", 1);
- l_plan_pub_ = ros_node.advertise<visualization_msgs::Polyline>("local_plan", 1);
+ footprint_pub_ = ros_node.advertise<geometry_msgs::PolygonStamped>("robot_footprint", 1);
+ g_plan_pub_ = ros_node.advertise<nav_msgs::Path>("global_plan", 1);
+ l_plan_pub_ = ros_node.advertise<nav_msgs::Path>("local_plan", 1);
- //@todo TODO: Remove old publishers with Polyline stuff
- new_footprint_pub_ = ros_node.advertise<geometry_msgs::PolygonStamped>("robot_footprint_new", 1);
- new_g_plan_pub_ = ros_node.advertise<nav_msgs::Path>("global_plan_new", 1);
- new_l_plan_pub_ = ros_node.advertise<nav_msgs::Path>("local_plan_new", 1);
-
global_frame_ = costmap_ros_->getGlobalFrameID();
robot_base_frame_ = costmap_ros_->getBaseFrameID();
ros_node.param("prune_plan", prune_plan_, true);
@@ -352,8 +345,8 @@
unsigned int needed_path_length = std::max(costmap_.getSizeInCellsX(), costmap_.getSizeInCellsY());
for(unsigned int i = 0; i < std::min((unsigned int)global_plan_.size(), needed_path_length); ++i){
- const geometry_msgs::PoseStamped& new_pose = global_plan_[i];
- poseStampedMsgToTF(new_pose, tf_pose);
+ const geometry_msgs::PoseStamped& pose = global_plan_[i];
+ poseStampedMsgToTF(pose, tf_pose);
tf_pose.setData(transform * tf_pose);
tf_pose.stamp_ = transform.stamp_;
tf_pose.frame_id_ = global_frame_;
@@ -469,8 +462,8 @@
//publish the robot footprint and an empty plan because we've reached our goal position
publishFootprint(global_pose);
- publishPlan(transformed_plan, g_plan_pub_, new_g_plan_pub_, 0.0, 1.0, 0.0, 0.0);
- publishPlan(local_plan, l_plan_pub_, new_l_plan_pub_, 0.0, 0.0, 1.0, 0.0);
+ publishPlan(transformed_plan, g_plan_pub_, 0.0, 1.0, 0.0, 0.0);
+ publishPlan(local_plan, l_plan_pub_, 0.0, 0.0, 1.0, 0.0);
//we don't actually want to run the controller when we're just rotating to goal
return true;
@@ -500,8 +493,8 @@
if(path.cost_ < 0){
local_plan.clear();
publishFootprint(global_pose);
- publishPlan(transformed_plan, g_plan_pub_, new_g_plan_pub_, 0.0, 1.0, 0.0, 0.0);
- publishPlan(local_plan, l_plan_pub_, new_l_plan_pub_, 0.0, 0.0, 1.0, 0.0);
+ publishPlan(transformed_plan, g_plan_pub_, 0.0, 1.0, 0.0, 0.0);
+ publishPlan(local_plan, l_plan_pub_, 0.0, 0.0, 1.0, 0.0);
return false;
}
@@ -518,8 +511,8 @@
//publish information to the visualizer
publishFootprint(global_pose);
- publishPlan(transformed_plan, g_plan_pub_, new_g_plan_pub_, 0.0, 1.0, 0.0, 0.0);
- publishPlan(local_plan, l_plan_pub_, new_l_plan_pub_, 0.0, 0.0, 1.0, 0.0);
+ publishPlan(transformed_plan, g_plan_pub_, 0.0, 1.0, 0.0, 0.0);
+ publishPlan(local_plan, l_plan_pub_, 0.0, 0.0, 1.0, 0.0);
return true;
}
@@ -563,34 +556,19 @@
footprint_poly.header.stamp = ros::Time::now();
footprint_poly.polygon.set_points_size(footprint.size());
- visualization_msgs::Polyline footprint_msg;
- footprint_msg.header.frame_id = global_frame_;
- footprint_msg.set_points_size(footprint.size());
- footprint_msg.color.r = 1.0;
- footprint_msg.color.g = 0;
- footprint_msg.color.b = 0;
- footprint_msg.color.a = 0;
for(unsigned int i = 0; i < footprint.size(); ++i){
- footprint_msg.points[i].x = footprint[i].x;
- footprint_msg.points[i].y = footprint[i].y;
- footprint_msg.points[i].z = footprint[i].z;
-
footprint_poly.polygon.points[i].x = footprint[i].x;
footprint_poly.polygon.points[i].y = footprint[i].y;
footprint_poly.polygon.points[i].z = footprint[i].z;
}
- footprint_pub_.publish(footprint_msg);
- new_footprint_pub_.publish(footprint_poly);
+ footprint_pub_.publish(footprint_poly);
}
- void TrajectoryPlannerROS::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, const ros::Publisher& pub, const ros::Publisher& new_pub, double r, double g, double b, double a){
+ void TrajectoryPlannerROS::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, const ros::Publisher& pub, double r, double g, double b, double a){
//given an empty path we won't do anything
if(path.empty())
return;
- visualization_msgs::Polyline gui_path_msg;
- gui_path_msg.header.frame_id = global_frame_;
-
//create a path message
nav_msgs::Path gui_path;
gui_path.set_poses_size(path.size());
@@ -598,24 +576,12 @@
gui_path.header.stamp = path[0].header.stamp;
// Extract the plan in world co-ordinates, we assume the path is all in the same frame
- gui_path_msg.header.stamp = path[0].header.stamp;
- gui_path_msg.set_points_size(path.size());
for(unsigned int i=0; i < path.size(); i++){
- gui_path_msg.points[i].x = path[i].pose.position.x;
- gui_path_msg.points[i].y = path[i].pose.position.y;
- gui_path_msg.points[i].z = path[i].pose.position.z;
-
gui_path.poses[i].pose.position.x = path[i].pose.position.x;
gui_path.poses[i].pose.position.y = path[i].pose.position.y;
gui_path.poses[i].pose.position.z = path[i].pose.position.z;
}
- gui_path_msg.color.r = r;
- gui_path_msg.color.g = g;
- gui_path_msg.color.b = b;
- gui_path_msg.color.a = a;
-
- pub.publish(gui_path_msg);
- new_pub.publish(gui_path);
+ pub.publish(gui_path);
}
};
Modified: pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/costmap_2d_publisher.h
===================================================================
--- pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/costmap_2d_publisher.h 2009-08-12 20:29:45 UTC (rev 21699)
+++ pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/costmap_2d_publisher.h 2009-08-12 20:32:42 UTC (rev 21700)
@@ -39,7 +39,6 @@
#include <ros/ros.h>
#include <ros/console.h>
#include <costmap_2d/costmap_2d.h>
-#include <visualization_msgs/Polyline.h>
#include <nav_msgs/GridCells.h>
#include <boost/thread.hpp>
@@ -88,8 +87,7 @@
std::vector< std::pair<double, double> > raw_obstacles_, inflated_obstacles_;
boost::recursive_mutex lock_; ///< @brief A lock
bool active_, new_data_;
- ros::Publisher raw_obs_pub_, inf_obs_pub_;
- ros::Publisher new_obs_pub_, new_inf_obs_pub_;
+ ros::Publisher obs_pub_, inf_obs_pub_;
double resolution_;
};
};
Modified: pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp
===================================================================
--- pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp 2009-08-12 20:29:45 UTC (rev 21699)
+++ pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp 2009-08-12 20:32:42 UTC (rev 21700)
@@ -40,12 +40,10 @@
Costmap2DPublisher::Costmap2DPublisher(ros::NodeHandle& ros_node, double publish_frequency, std::string global_frame)
: ros_node_(ros_node), global_frame_(global_frame),
visualizer_thread_(NULL), active_(false), new_data_(false), resolution_(0.0){
- //@todo TODO: Remove old obstacle publishers
- raw_obs_pub_ = ros_node_.advertise<visualization_msgs::Polyline>("raw_obstacles", 1);
- inf_obs_pub_ = ros_node_.advertise<visualization_msgs::Polyline>("inflated_obstacles", 1);
- new_obs_pub_ = ros_node_.advertise<nav_msgs::GridCells>("obstacles_new", 1);
- new_inf_obs_pub_ = ros_node_.advertise<nav_msgs::GridCells>("inflated_obstacles_new", 1);
+ obs_pub_ = ros_node_.advertise<nav_msgs::GridCells>("obstacles", 1);
+ inf_obs_pub_ = ros_node_.advertise<nav_msgs::GridCells>("inflated_obstacles", 1);
+
visualizer_thread_ = new boost::thread(boost::bind(&Costmap2DPublisher::mapPublishLoop, this, publish_frequency));
}
@@ -110,17 +108,7 @@
resolution = resolution_;
lock_.unlock();
- //@todo TODO: Remove Polyline generation
-
- // First publish raw obstacles in red
- visualization_msgs::Polyline obstacle_msg;
- obstacle_msg.header.frame_id = global_frame_;
unsigned int point_count = raw_obstacles.size();
- obstacle_msg.set_points_size(point_count);
- obstacle_msg.color.a = 0.0;
- obstacle_msg.color.r = 1.0;
- obstacle_msg.color.b = 0.0;
- obstacle_msg.color.g = 0.0;
//create a GridCells message for the obstacles
nav_msgs::GridCells obstacle_cells;
@@ -135,42 +123,24 @@
obstacle_cells.set_cells_size(point_count);
for(unsigned int i=0;i<point_count;i++){
- obstacle_msg.points[i].x = raw_obstacles[i].first;
- obstacle_msg.points[i].y = raw_obstacles[i].second;
- obstacle_msg.points[i].z = 0;
-
obstacle_cells.cells[i].x = raw_obstacles[i].first;
obstacle_cells.cells[i].y = raw_obstacles[i].second;
obstacle_cells.cells[i].z = 0;
}
- raw_obs_pub_.publish(obstacle_msg);
- new_obs_pub_.publish(obstacle_cells);
+ obs_pub_.publish(obstacle_cells);
- // Now do inflated obstacles in blue
- point_count = inflated_obstacles.size();
- obstacle_msg.set_points_size(point_count);
- obstacle_msg.color.a = 0.0;
- obstacle_msg.color.r = 0.0;
- obstacle_msg.color.b = 1.0;
- obstacle_msg.color.g = 0.0;
-
//set the size equal to the number of inflated obstacles
+ point_count = inflated_obstacles.size();
obstacle_cells.set_cells_size(point_count);
for(unsigned int i=0;i<point_count;i++){
- obstacle_msg.points[i].x = inflated_obstacles[i].first;
- obstacle_msg.points[i].y = inflated_obstacles[i].second;
- obstacle_msg.points[i].z = 0;
-
obstacle_cells.cells[i].x = inflated_obstacles[i].first;
obstacle_cells.cells[i].y = inflated_obstacles[i].second;
obstacle_cells.cells[i].z = 0;
}
- inf_obs_pub_.publish(obstacle_msg);
-
- new_inf_obs_pub_.publish(obstacle_cells);
+ inf_obs_pub_.publish(obstacle_cells);
}
};
Deleted: pkg/trunk/stacks/navigation/costmap_2d/src/costmap_test.cpp
===================================================================
--- pkg/trunk/stacks/navigation/costmap_2d/src/costmap_test.cpp 2009-08-12 20:29:45 UTC (rev 21699)
+++ pkg/trunk/stacks/navigation/costmap_2d/src/costmap_test.cpp 2009-08-12 20:32:42 UTC (rev 21700)
@@ -1,287 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-#include <ros/node.h>
-#include <ros/console.h>
-#include <new_costmap/costmap_2d.h>
-#include <new_costmap/observation_buffer.h>
-#include <nav_msgs/GetMap.h>
-#include <visualization_msgs/Polyline.h>
-#include <map>
-#include <vector>
-
-#include <tf/transform_datatypes.h>
-#include <tf/message_notifier.h>
-#include <tf/transform_listener.h>
-
-#include <sensor_msgs/LaserScan.h>
-#include <laser_scan/laser_scan.h>
-
-#include <sensor_msgs/PointCloud.h>
-
-// Thread suppport
-#include <boost/thread.hpp>
-
-using namespace costmap_2d;
-using namespace tf;
-using sensor_msgs::PointCloud;
-
-class CostmapTester {
- public:
- CostmapTester(ros::Node& ros_node) : ros_node_(ros_node), base_scan_notifier_(NULL), tf_(ros_node, true, ros::Duration(10)), global_frame_("map"), freq_(5), base_scan_buffer_(NULL) {
- ros_node.advertise<visualization_msgs::Polyline>("raw_obstacles", 1);
- ros_node.advertise<visualization_msgs::Polyline>("inflated_obstacles", 1);
-
- base_scan_buffer_ = new ObservationBuffer(0.0, 0.2, tf_, "map", "base_laser");
-
- base_scan_notifier_ = new MessageNotifier<sensor_msgs::LaserScan>(&tf_, &ros_node,
- boost::bind(&CostmapTester::baseScanCallback, this, _1, (int) 1),
- "base_scan", global_frame_, 50);
-
- nav_msgs::GetMap::Request map_req;
- nav_msgs::GetMap::Response map_resp;
- ROS_INFO("Requesting the map...\n");
- while(!ros::service::call("static_map", map_req, map_resp))
- {
- ROS_INFO("Request failed; trying again...\n");
- usleep(1000000);
- }
- ROS_INFO("Received a %d X %d map at %f m/pix\n",
- map_resp.map.info.width, map_resp.map.info.height, map_resp.map.info.resolution);
-
- // We are treating cells with no information as lethal obstacles based on the input data. This is not ideal but
- // our planner and controller do not reason about the no obstacle case
- std::vector<unsigned char> input_data;
- unsigned int numCells = map_resp.map.info.width * map_resp.map.info.height;
- for(unsigned int i = 0; i < numCells; i++){
- input_data.push_back((unsigned char) map_resp.map.data[i]);
- }
-
- struct timeval start, end;
- double start_t, end_t, t_diff;
- gettimeofday(&start, NULL);
- new_costmap_ = new Costmap2D((unsigned int)map_resp.map.info.width, (unsigned int)map_resp.map.info.height,
- map_resp.map.info.resolution, 0.0, 0.0, 0.325, 0.46, 0.55, 2.5, 2.0, 3.0, 1.0, input_data, 100);
- gettimeofday(&end, NULL);
- start_t = start.tv_sec + double(start.tv_usec) / 1e6;
- end_t = end.tv_sec + double(end.tv_usec) / 1e6;
- t_diff = end_t - start_t;
- ROS_INFO("New map construction time: %.9f", t_diff);
-
-
- //create a separate thread to publish cost data to the visualizer
- visualizer_thread_ = new boost::thread(boost::bind(&CostmapTester::publishCostmap, this));
- window_reset_thread_ = new boost::thread(boost::bind(&CostmapTester::resetWindow, this));
-
- }
-
- ~CostmapTester(){
- delete new_costmap_;
- delete base_scan_notifier_;
- delete visualizer_thread_;
- delete window_reset_thread_;
- delete base_scan_buffer_;
- }
-
- void baseScanCallback(const tf::MessageNotifier<sensor_msgs::LaserScan>::MessagePtr& message, int i){
- //project the laser into a point cloud
- PointCloud base_cloud;
- base_cloud.header = message->header;
- //we want all values... even those out of range
- projector_.projectLaser(*message, base_cloud, -1.0, true);
-
- //buffer the point cloud
- lock_.lock();
- base_scan_buffer_->bufferCloud(base_cloud);
- lock_.unlock();
- }
-
- void spin(){
- while(ros_node_.ok()){
- updateMap();
- usleep(1e6/freq_);
- }
- }
-
- void updateMap(){
- tf::Stamped<tf::Pose> robot_pose, global_pose;
- global_pose.setIdentity();
- robot_pose.setIdentity();
- robot_pose.frame_id_ = "base_link";
- robot_pose.stamp_ = ros::Time();
- try{
- tf_.transformPose(global_frame_, robot_pose, global_pose);
- }
- catch(tf::LookupException& ex) {
- ROS_ERROR("No Transform available Error: %s\n", ex.what());
- }
- catch(tf::ConnectivityException& ex) {
- ROS_ERROR("Connectivity Error: %s\n", ex.what());
- }
- catch(tf::ExtrapolationException& ex) {
- ROS_ERROR("Extrapolation Error: %s\n", ex.what());
- }
-
- double wx = global_pose.getOrigin().x();
- double wy = global_pose.getOrigin().y();
-
- //in the real world... make a deep copy... but for testing I'm lazy so I'll lock around the updates
- lock_.lock();
- struct timeval start, end;
- double start_t, end_t, t_diff;
- gettimeofday(&start, NULL);
- std::vector<Observation> observations;
- base_scan_buffer_->getObservations(observations);
- new_costmap_->updateWorld(wx, wy, observations, observations);
- gettimeofday(&end, NULL);
- start_t = start.tv_sec + double(start.tv_usec) / 1e6;
- end_t = end.tv_sec + double(end.tv_usec) / 1e6;
- t_diff = end_t - start_t;
- ROS_INFO("Map update time: %.9f", t_diff);
- lock_.unlock();
- }
-
- void resetWindow(){
- while(ros_node_.ok()){
- tf::Stamped<tf::Pose> robot_pose, global_pose;
- global_pose.setIdentity();
- robot_pose.setIdentity();
- robot_pose.frame_id_ = "base_link";
- robot_pose.stamp_ = ros::Time();
- try{
- tf_.transformPose(global_frame_, robot_pose, global_pose);
- }
- catch(tf::LookupException& ex) {
- ROS_ERROR("No Transform available Error: %s\n", ex.what());
- }
- catch(tf::ConnectivityException& ex) {
- ROS_ERROR("Connectivity Error: %s\n", ex.what());
- }
- catch(tf::ExtrapolationException& ex) {
- ROS_ERROR("Extrapolation Error: %s\n", ex.what());
- }
-
- double wx = global_pose.getOrigin().x();
- double wy = global_pose.getOrigin().y();
- lock_.lock();
- ROS_INFO("Resetting map outside window");
- new_costmap_->resetMapOutsideWindow(wx, wy, 5.0, 5.0);
- lock_.unlock();
-
- usleep(1e6/0.2);
- }
- }
-
- void publishCostmap(){
- while(ros_node_.ok()){
- ROS_INFO("publishing map");
- lock_.lock();
- std::vector< std::pair<double, double> > raw_obstacles, inflated_obstacles;
- for(unsigned int i = 0; i<new_costmap_->cellSizeX(); i++){
- for(unsigned int j = 0; j<new_costmap_->cellSizeY();j++){
- double wx, wy;
- new_costmap_->mapToWorld(i, j, wx, wy);
- std::pair<double, double> p(wx, wy);
-
- if(new_costmap_->getCost(i, j) == costmap_2d::LETHAL_OBSTACLE)
- raw_obstacles.push_back(p);
- else if(new_costmap_->getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
- inflated_obstacles.push_back(p);
- }
- }
- lock_.unlock();
-
- // First publish raw obstacles in red
- visualization_msgs::Polyline obstacle_msg;
- obstacle_msg.header.frame_id = global_frame_;
- unsigned int pointCount = raw_obstacles.size();
- obstacle_msg.set_points_size(pointCount);
- obstacle_msg.color.a = 0.0;
- obstacle_msg.color.r = 1.0;
- obstacle_msg.color.b = 0.0;
- obstacle_msg.color.g = 0.0;
-
- for(unsigned int i=0;i<pointCount;i++){
- obstacle_msg.points[i].x = raw_obstacles[i].first;
- obstacle_msg.points[i].y = raw_obstacles[i].second;
- obstacle_msg.points[i].z = 0;
- }
-
- ros::Node::instance()->publish("raw_obstacles", obstacle_msg);
-
- // Now do inflated obstacles in blue
- pointCount = inflated_obstacles.size();
- obstacle_msg.set_points_size(pointCount);
- obstacle_msg.color.a = 0.0;
- obstacle_msg.color.r = 0.0;
- obstacle_msg.color.b = 1.0;
- obstacle_msg.color.g = 0.0;
-
- for(unsigned int i=0;i<pointCount;i++){
- obstacle_msg.points[i].x = inflated_obstacles[i].first;
- obstacle_msg.points[i].y = inflated_obstacles[i].second;
- obstacle_msg.points[i].z = 0;
- }
-
- ros::Node::instance()->publish("inflated_obstacles", obstacle_msg);
- usleep(1e6/2.0);
- }
- }
-
- ros::Node& ros_node_;
- tf::MessageNotifier<sensor_msgs::LaserScan>* base_scan_notifier_; ///< @brief Used to guarantee that a transform is available for base scans
- tf::TransformListener tf_; ///< @brief Used for transforming point clouds
- laser_scan::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds
- boost::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely
- Costmap2D* new_costmap_;
- std::string global_frame_;
- double freq_;
- boost::thread* visualizer_thread_;
- boost::thread* window_reset_thread_;
- ObservationBuffer* base_scan_buffer_;
-
-};
-
-int main(int argc, char** argv){
- ros::init(argc, argv);
- ros::Node ros_node("new_costmap_tester");
- CostmapTester tester(ros_node);
- tester.spin();
-
- return(0);
-
-}
-
Modified: pkg/trunk/stacks/navigation/navfn/include/navfn/navfn_ros.h
===================================================================
--- pkg/trunk/stacks/navigation/navfn/include/navfn/navfn_ros.h 2009-08-12 20:29:45 UTC (rev 21699)
+++ pkg/trunk/stacks/navigation/navfn/include/navfn/navfn_ros.h 2009-08-12 20:32:42 UTC (rev 21700)
@@ -42,7 +42,6 @@
#include <costmap_2d/costmap_2d_ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>
-#include <visualization_msgs/Polyline.h>
#include <nav_msgs/Path.h>
#include <tf/transform_datatypes.h>
#include <vector>
@@ -118,7 +117,6 @@
boost::shared_ptr<NavFn> planner_;
double inscribed_radius_, circumscribed_radius_, inflation_radius_;
ros::Publisher plan_pub_;
- ros::Publisher new_plan_pub_;
bool initialized_;
Modified: pkg/trunk/stacks/navigation/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/stacks/navigation/navfn/src/navfn_ros.cpp 2009-08-12 20:29:45 UTC (rev ...
[truncated message content] |
