Add carrot follower example

leggedrobotics · Apr 10, 2024 · 88a3b1e · 88a3b1e
1 parent dd6aa2e
commit 88a3b1e
Show file tree

Hide file tree

Showing 6 changed files with 200 additions and 17 deletions.
diff --git a/wild_visual_navigation_jackal/CMakeLists.txt b/wild_visual_navigation_jackal/CMakeLists.txt
@@ -21,6 +21,7 @@ install(DIRECTORY launch Media worlds
 ############
 ## Python ##
 ############
-catkin_install_python(PROGRAMS scripts/jackal_state_converter_node.py
+catkin_install_python(PROGRAMS scripts/carrot_follower.py
+                               scripts/jackal_state_converter_node.py
                                scripts/gazebo_world_publisher.py
                       DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
diff --git a/wild_visual_navigation_jackal/config/rviz/open_source.rviz b/wild_visual_navigation_jackal/config/rviz/open_source.rviz
@@ -7,7 +7,6 @@ Panels:
         - /Global Options1
         - /TF1/Tree1
         - /World1/Status1
-        - /Robot Info1
         - /Wild Visual Navigation1
         - /Wild Visual Navigation1/Self Supervision1
         - /Wild Visual Navigation1/Cameras Resized1
@@ -65,6 +64,8 @@ Visualization Manager:
       Frame Timeout: 1000
       Frames:
         All Enabled: false
+        base:
+          Value: true
         base_link:
           Value: true
         chassis_link:
@@ -97,13 +98,17 @@ Visualization Manager:
           Value: true
         rear_right_wheel_link:
           Value: true
+        world:
+          Value: true
       Marker Alpha: 1
       Marker Scale: 0.5
       Name: TF
       Show Arrows: true
       Show Axes: true
       Show Names: true
       Tree:
+        base:
+          {}
         odom:
           base_link:
             chassis_link:
@@ -130,6 +135,8 @@ Visualization Manager:
                 {}
               rear_right_wheel_link:
                 {}
+            world:
+              {}
       Update Interval: 0
       Value: true
     - Class: rviz/Marker
@@ -795,11 +802,27 @@ Visualization Manager:
           Value: false
       Enabled: true
       Name: GridMaps
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Queue Size: 10
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic: /move_base_simple/goal
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 255; 255; 255
     Default Light: true
-    Fixed Frame: odom
+    Fixed Frame: world
     Frame Rate: 30
   Name: root
   Tools:
@@ -823,25 +846,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/ThirdPersonFollower
-      Distance: 19.5626220703125
+      Distance: 47.735103607177734
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 6.84149169921875
-        Y: -0.8593425750732422
-        Z: 7.152557373046875e-07
+        X: 6.462728023529053
+        Y: -0.07755613327026367
+        Z: 9.5367431640625e-07
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.6603981852531433
+      Pitch: 0.7597965598106384
       Target Frame: base
-      Yaw: 5.903587341308594
+      Yaw: 6.278591632843018
     Saved: ~
 Window Geometry:
   AB Wide Angle Front:
@@ -863,7 +886,7 @@ Window Geometry:
     collapsed: false
   Learning Mask:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   R Confidence Overlay:
     collapsed: false
   R Traversability Overlay:

diff --git a/wild_visual_navigation_jackal/launch/sim.launch b/wild_visual_navigation_jackal/launch/sim.launch
@@ -8,7 +8,7 @@
 
     <arg name="x" default="0.0" />
     <arg name="y" default="5.0" />
-    <arg name="z" default="1.0" />
+    <arg name="z" default="0.0" />
     <arg name="yaw" default="0" />
 
     <!-- Launch Gazebo with the specified world -->
@@ -29,7 +29,9 @@
 
   <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
 	      args="-urdf -model jackal -param robot_description -x $(arg x) -y $(arg y) -z $(arg z) -R 0 -P 0 -Y $(arg yaw)" />
-
-  <node name="gazebo_world_publisher" pkg="wild_visual_navigation_jackal" type="gazebo_world_publisher.py"/>
 
+  <!-- Simulation demo helpers -->
+  <node name="gazebo_world_publisher" pkg="wild_visual_navigation_jackal" type="gazebo_world_publisher.py"/>
+  <node name="gazebo_world_publisher" pkg="wild_visual_navigation_jackal" type="carrot_follower.py"/>
+
 </launch>