Feat/roboasis requirements

robotnik_gazebo_ignition/hooks/robotnik_gazebo_ignition.sh.in

@@ -28,12 +28,23 @@ for _prefix in $(printf %s "$_prefixes" | tr ':' ' '); do
   for _pkg in $_GZ_PKGS; do
     _p="$_prefix/share/$_pkg"
     if [ -d "$_p" ]; then
-      _prepend_unique_value GZ_SIM_RESOURCE_PATH "$(_get_parent_abs_path "$_p")"
-      _prepend_unique_value IGN_GAZEBO_RESOURCE_PATH "$(_get_parent_abs_path "$_p")"
-      _prepend_unique_value GAZEBO_RESOURCE_PATH "$(_get_parent_abs_path "$_p")"
+      _parent_path="$(_get_parent_abs_path "$_p")"
+      _model_path="${_parent_path}/$_pkg/models"
+
+      # Always add the parent path
+      _prepend_unique_value GZ_SIM_RESOURCE_PATH "$_parent_path"
+      _prepend_unique_value IGN_GAZEBO_RESOURCE_PATH "$_parent_path"
+      _prepend_unique_value GAZEBO_RESOURCE_PATH "$_parent_path"
+
+      # If 'models' subfolder exists, add that too
+      if [ -d "$_model_path" ]; then
+        _prepend_unique_value GZ_SIM_RESOURCE_PATH "$_model_path"
+        _prepend_unique_value IGN_GAZEBO_RESOURCE_PATH "$_model_path"
+        _prepend_unique_value GAZEBO_RESOURCE_PATH "$_model_path"
+      fi
     fi
   done
 done
 
-unset _prefix _pkg _p _prefixes _self_prefix _hook_dir _GZ_PKGS
+unset _prefix _pkg _p _prefixes _self_prefix _hook_dir _GZ_PKGS _parent_path _model_path
 unset -f _prepend_unique_value _get_parent_abs_path

robotnik_gazebo_ignition/launch/spawn_robot.launch.py

@@ -175,14 +175,19 @@ def add_depth_camera(camera_name):
                 (f"/{robot_id}/{camera_name}_camera_depth/depth/camera_info", f"/{robot_id}/{camera_name}_rgbd_camera/depth/camera_info", "sensor_msgs/msg/CameraInfo", "gz.msgs.CameraInfo", "GZ_TO_ROS"),
                 (f"/{robot_id}/{camera_name}_camera_depth/depth/image_raw", f"/{robot_id}/{camera_name}_rgbd_camera/depth/image_raw", "sensor_msgs/msg/Image", "gz.msgs.Image", "GZ_TO_ROS"),
             ])
-
+        # Pose from gz simulator
+        def add_pose():
+            bridge_raw.extend([
+                ( f"/model/robot/pose", f"/model/robot/pose", "geometry_msgs/msg/PoseStamped", "gz.msgs.Pose", "GZ_TO_ROS"),
+            ])
         add_camera("front")
         add_camera("rear")
         add_camera("top_ptz")
         #add_depth_camera("front")
         add_laser("front")
         add_laser("rear")
         add_pointcloud("top")
+        add_pose()
 
         bridge_config = [{"ros_topic_name": ros, "gz_topic_name": gz, "ros_type_name": ros_type, "gz_type_name": gz_type, "direction": direction} for gz, ros, ros_type, gz_type, direction in bridge_raw]
         with tempfile.NamedTemporaryFile(mode='w', delete=False) as tmp:
@@ -271,12 +276,13 @@ def get_ros2_control_yaml_path(params):
         executable="rviz2",
         namespace=params['robot_id'],
         arguments=[
-            '-d', [FindPackageShare('robotnik_gazebo_ignition'), '/config/rviz_config.rviz'],
+            '-d', [params['rviz_config']],
             # Fixed frame
-            '-f', [params['robot_id'], '_odom'],
+            # '-f', [params['robot_id'], '_odom'],
             # Window name
             '-t', [params['robot_id'], ' - ', params['robot_model'], ' - RViz'],
         ],
+        parameters=[{'use_sim_time': True}],
         condition=IfCondition(params['run_rviz'])
     ))
     return ret
@@ -288,11 +294,13 @@ def generate_launch_description():
         ("robot", "Robot Model Name", "", "ROBOT"),
         ("robot_model", "Robot Variant or Type", LaunchConfiguration('robot'), "ROBOT_MODEL"),
         ("robot_xacro", "Path to Robot Xacro File", [FindPackageShare('robotnik_description'), '/robots/', LaunchConfiguration('robot'), '/', LaunchConfiguration('robot_model'), '.urdf.xacro'], "ROBOT_XACRO"),
-        ("x", "Initial X Coordinate", "0.0", "X"),
-        ("y", "Initial Y Coordinate", "0.0", "Y"),
-        ("z", "Initial Z Coordinate", "0.0", "Z"),
+        ("x", "Initial X Coordinate", "-11.058", "X"),
+        ("y", "Initial Y Coordinate", "-78.487", "Y"),
+        ("z", "Initial Z Coordinate", "0.05", "Z"),
         ("has_arm", "Enable Arm Controller", "False", "HAS_ARM"),
         ("run_rviz", "Run RViz", "True", "RUN_RVIZ"),
+        ("use_sim_time", "Use simulation time", "True", "USE_SIM_TIME"),
+        ("rviz_config", "RViz configuration file", [FindPackageShare('robotnik_gazebo_ignition'), '/config/rviz_config.rviz'], "CONFIG_RVIZ"),
     ]
 
     ld = LaunchDescription()