Ongoing fixes to visualize world in RViz

wild_visual_navigation_jackal/CMakeLists.txt

@@ -13,14 +13,6 @@ catkin_package(
   CATKIN_DEPENDS
 )
 
-# catkin_python_setup()
-# catkin_install_python(PROGRAMS  scripts/wvn_feature_extractor_node.py
-#                                 scripts/wvn_learning_node.py
-#                                 scripts/overlay_images.py
-#                                 scripts/smart_carrot.py
-#                                 scripts/rosbag_play.sh
-#                       DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
-
 message(${CATKIN_PACKAGE_SHARE_DESTINATION})
 install(DIRECTORY launch Media worlds
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
@@ -30,4 +22,5 @@ install(DIRECTORY launch Media worlds
 ## Python ##
 ############
 catkin_install_python(PROGRAMS scripts/jackal_state_converter_node.py
+                               scripts/gazebo_world_publisher.py
                       DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

wild_visual_navigation_jackal/launch/sim.launch

@@ -29,4 +29,7 @@
 
   <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"/>
+
 </launch>

wild_visual_navigation_jackal/scripts/gazebo_world_publisher.py

@@ -0,0 +1,46 @@
+#!/usr/bin/python3
+#
+# Copyright (c) 2022-2024, ETH Zurich, Matias Mattamala, Jonas Frey.
+# All rights reserved. Licensed under the MIT license.
+# See LICENSE file in the project root for details.
+#
+
+from visualization_msgs.msg import Marker
+import rospkg
+import rospy
+
+
+def callback(event):
+    pub.publish(marker)
+
+
+if __name__ == "__main__":
+    rospy.init_node("gazebo_world_publisher")
+
+    # Default variables
+    rospack = rospkg.RosPack()
+    pkg_path = rospack.get_path("wild_visual_navigation_jackal")
+    default_model_file = f"{pkg_path}/Media/models/outdoor.dae"
+
+    marker = Marker()
+    marker.header.frame_id = "odom"
+    marker.header.stamp = rospy.Time.now()
+    marker.id = 0
+    marker.ns = "world"
+    marker.action = Marker.ADD
+    marker.scale.x = 1.0
+    marker.scale.y = 1.0
+    marker.scale.z = 1.0
+    marker.color.a = 1.0
+    marker.pose.orientation.w = 1.0
+    marker.mesh_use_embedded_materials = True
+    marker.type = Marker.MESH_RESOURCE
+    marker.mesh_resource = f"file://{default_model_file}"
+
+    # Set publisher
+    pub = rospy.Publisher("/wild_visual_navigation_jackal/simulation_world", Marker, queue_size=10)
+
+    # Set timer to publish
+    rospy.Timer(rospy.Duration(5), callback)
+    rospy.loginfo("[gazebo_world_publisher] Published world!")
+    rospy.spin()