Relay move_base plan names

jsk-ros-pkg · Jul 15, 2021 · 93ee663 · 93ee663
1 parent 78abf9b
commit 93ee663
Show file tree

Hide file tree

Showing 6 changed files with 34 additions and 18 deletions.
diff --git a/jsk_fetch_robot/jsk_fetch_startup/apps/go_to_kitchen/go_to_kitchen.app b/jsk_fetch_robot/jsk_fetch_startup/apps/go_to_kitchen/go_to_kitchen.app
@@ -71,12 +71,9 @@ plugins:
         - /odom
         - /odom_combined
         - /cmd_vel
-        - /move_base/NavFnROS/plan
-        - /move_base/TrajectoryPlannerROS/global_plan
-        - /move_base/TrajectoryPlannerROS/local_plan
-        - /move_base/GlobalPlanner/plan
-        - /move_base/TebLocalPlannerROS/global_plan
-        - /move_base/TebLocalPlannerROS/local_plan
+        - /move_base/navigation_plan_viz
+        - /move_base/global_plan_viz
+        - /move_base/local_plan_viz
         - /move_base/global_costmap/footprint
         - /spots_marker_array
         - /spots_pictogram

diff --git a/jsk_fetch_robot/jsk_fetch_startup/config/jsk_startup.rviz b/jsk_fetch_robot/jsk_fetch_startup/config/jsk_startup.rviz
@@ -240,7 +240,7 @@ Visualization Manager:
       Radius: 0.029999999329447746
       Shaft Diameter: 0.10000000149011612
       Shaft Length: 0.10000000149011612
-      Topic: /move_base/NavfnROS/plan
+      Topic: /move_base/navigation_plan_viz
       Unreliable: false
       Value: true
     - Alpha: 1
@@ -263,7 +263,7 @@ Visualization Manager:
       Radius: 0.029999999329447746
       Shaft Diameter: 0.10000000149011612
       Shaft Length: 0.10000000149011612
-      Topic: /move_base/TebLocalPlannerROS/global_plan
+      Topic: /move_base/global_plan_viz
       Unreliable: false
       Value: true
     - Alpha: 1
@@ -286,7 +286,7 @@ Visualization Manager:
       Radius: 0.029999999329447746
       Shaft Diameter: 0.10000000149011612
       Shaft Length: 0.10000000149011612
-      Topic: /move_base/TebLocalPlannerROS/local_plan
+      Topic: /move_base/local_plan_viz
       Unreliable: false
       Value: true
     - Alpha: 1

diff --git a/jsk_fetch_robot/jsk_fetch_startup/config/jsk_startup_record.rviz b/jsk_fetch_robot/jsk_fetch_startup/config/jsk_startup_record.rviz
@@ -258,7 +258,7 @@ Visualization Manager:
       Radius: 0.029999999329447746
       Shaft Diameter: 0.10000000149011612
       Shaft Length: 0.10000000149011612
-      Topic: /move_base/NavfnROS/plan
+      Topic: /move_base/navigation_plan_viz
       Unreliable: false
       Value: true
     - Alpha: 1
@@ -281,7 +281,7 @@ Visualization Manager:
       Radius: 0.029999999329447746
       Shaft Diameter: 0.10000000149011612
       Shaft Length: 0.10000000149011612
-      Topic: /move_base/TebLocalPlannerROS/global_plan
+      Topic: /move_base/global_plan_viz
       Unreliable: false
       Value: true
     - Alpha: 1
@@ -304,7 +304,7 @@ Visualization Manager:
       Radius: 0.029999999329447746
       Shaft Diameter: 0.10000000149011612
       Shaft Length: 0.10000000149011612
-      Topic: /move_base/TebLocalPlannerROS/local_plan
+      Topic: /move_base/local_plan_viz
       Unreliable: false
       Value: true
     - Alpha: 1

diff --git a/jsk_fetch_robot/jsk_fetch_startup/config/lightweight_navigation.rviz b/jsk_fetch_robot/jsk_fetch_startup/config/lightweight_navigation.rviz
@@ -235,7 +235,7 @@ Visualization Manager:
       Radius: 0.029999999329447746
       Shaft Diameter: 0.10000000149011612
       Shaft Length: 0.10000000149011612
-      Topic: /move_base/NavfnROS/plan
+      Topic: /move_base/navigation_plan_viz
       Unreliable: false
       Value: true
     - Alpha: 1
@@ -258,7 +258,7 @@ Visualization Manager:
       Radius: 0.029999999329447746
       Shaft Diameter: 0.10000000149011612
       Shaft Length: 0.10000000149011612
-      Topic: /move_base/TrajectoryPlannerROS/global_plan
+      Topic: /move_base/global_plan_viz
       Unreliable: false
       Value: true
     - Alpha: 1
@@ -281,7 +281,7 @@ Visualization Manager:
       Radius: 0.029999999329447746
       Shaft Diameter: 0.10000000149011612
       Shaft Length: 0.10000000149011612
-      Topic: /move_base/TrajectoryPlannerROS/local_plan
+      Topic: /move_base/local_plan_viz
       Unreliable: false
       Value: true
     - Alpha: 1

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -208,6 +208,25 @@
       <arg name="launch_map_server" value="false" />
       <arg name="odom_topic" value="/odom_combined" />
     </include>
+    <!-- relay plan path topic for visualization -->
+    <node name="relay_navfn_planner_plan"
+          pkg="topic_tools" type="relay"
+          args="/move_base/NavfnROS/plan /move_base/navigation_plan_viz" />
+    <node name="relay_global_planner_plan"
+          pkg="topic_tools" type="relay"
+          args="/move_base/GlobalPlanner/plan /move_base/navigation_plan_viz" />
+    <node name="relay_trajectory_planner_global_plan"
+          pkg="topic_tools" type="relay"
+          args="/move_base/TrajectoryPlannerROS/global_plan /move_base/global_plan_viz" />
+    <node name="relay_trajectory_planner_local_plan"
+          pkg="topic_tools" type="relay"
+          args="/move_base/TrajectoryPlannerROS/local_plan /move_base/local_plan_viz" />
+    <node name="relay_teb_planner_global_plan"
+          pkg="topic_tools" type="relay"
+          args="/move_base/TebLocalPlannerROS/global_plan /move_base/global_plan_viz" />
+    <node name="relay_teb_planner_local_plan"
+          pkg="topic_tools" type="relay"
+          args="/move_base/TebLocalPlannerROS/local_plan /move_base/local_plan_viz" />
 
     <!-- load amcl params -->
     <rosparam command="load"

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/rosbag_record.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/rosbag_record.launch
@@ -16,9 +16,9 @@
           /odom
           /odom_combined
           /cmd_vel
-          /move_base/NavFnROS/plan
-          /move_base/TrajectoryPlannerROS/global_plan
-          /move_base/TrajectoryPlannerROS/local_plan
+          /move_base/navigation_plan_viz
+          /move_base/global_plan_viz
+          /move_base/local_plan_viz
           /move_base/global_costmap/footprint
           /spots_marker_array
           /spots_pictogram