From 164cc3cc3fdb187832f1f8832f4ad5661a819b68 Mon Sep 17 00:00:00 2001
From: Bill Finger <ur.narmak@gmail.com>
Date: Thu, 20 Jan 2022 20:16:02 +0400
Subject: [PATCH] =?UTF-8?q?=F0=9F=94=A7=20add=20gazebo=5Fros=5Fstate=20plu?=
 =?UTF-8?q?gin?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 rasmt_support/CMakeLists.txt                |   1 +
 rasmt_support/launch/rasmt_gazebo.launch.py |  32 +++--
 rasmt_support/world/robossembler.world      | 123 ++++++++++++++++++++
 3 files changed, 146 insertions(+), 10 deletions(-)
 create mode 100644 rasmt_support/world/robossembler.world

diff --git a/rasmt_support/CMakeLists.txt b/rasmt_support/CMakeLists.txt
index 5cf06a7..d0ec2ae 100644
--- a/rasmt_support/CMakeLists.txt
+++ b/rasmt_support/CMakeLists.txt
@@ -35,5 +35,6 @@ install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY meshes DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY world DESTINATION share/${PROJECT_NAME})
 
 ament_package()
diff --git a/rasmt_support/launch/rasmt_gazebo.launch.py b/rasmt_support/launch/rasmt_gazebo.launch.py
index 13da416..086eb3d 100644
--- a/rasmt_support/launch/rasmt_gazebo.launch.py
+++ b/rasmt_support/launch/rasmt_gazebo.launch.py
@@ -18,15 +18,25 @@ def generate_launch_description():
         description="Set robot name in gazebo env"
     ))
 
-    # Launch Gazebo
-    gazebo = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            PathJoinSubstitution([
-                FindPackageShare("gazebo_ros"), 
-                "launch", 
-                "gazebo.launch.py"
-            ])
-        )
+    world_file = os.path.join(get_package_share_directory("rasmt_support"), "world", "robossembler.world")
+
+    gzserver = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                    PathJoinSubstitution([
+                            FindPackageShare("gazebo_ros"),
+                            "launch",
+                            "gzserver.launch.py"
+                    ])
+            ), launch_arguments={'world':world_file}.items()
+    )
+    gzclient = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                    PathJoinSubstitution([
+                            FindPackageShare("gazebo_ros"),
+                            "launch",
+                            "gzclient.launch.py"
+                    ])
+            )
     )
     
     """xacro_file = os.path.join(get_package_share_directory("rasmt_support"),"urdf/","rasmt.xacro")
@@ -55,7 +65,9 @@ def generate_launch_description():
     )"""
 
     launch_nodes = []
-    launch_nodes.append(gazebo)
+    #launch_nodes.append(gazebo)
+    launch_nodes.append(gzserver)
+    launch_nodes.append(gzclient)
     launch_nodes.append(spawn_entity)
     #launch_nodes.append(cube_spawn)
 
diff --git a/rasmt_support/world/robossembler.world b/rasmt_support/world/robossembler.world
new file mode 100644
index 0000000..bc8a33b
--- /dev/null
+++ b/rasmt_support/world/robossembler.world
@@ -0,0 +1,123 @@
+<sdf version='1.7'>
+  <world name='default'>
+    <light name='sun' type='directional'>
+      <cast_shadows>1</cast_shadows>
+      <pose>0 0 10 0 -0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+      <spot>
+        <inner_angle>0</inner_angle>
+        <outer_angle>0</outer_angle>
+        <falloff>0</falloff>
+      </spot>
+    </light>
+    <plugin name="gazebo_ros_state" filename="libgazebo_ros_state.so">
+        <ros>
+            <!--namespace>/rasmt</namespace-->
+            <argument>model_states:=model_states_demo</argument>
+        </ros>
+        <update_rate>1.0</update_rate>
+    </plugin>
+    <model name='ground_plane'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>100</mu>
+                <mu2>50</mu2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+    </model>
+    <gravity>0 0 -9.8</gravity>
+    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
+    <atmosphere type='adiabatic'/>
+    <physics type='ode'>
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>1000</real_time_update_rate>
+    </physics>
+    <scene>
+      <ambient>0.4 0.4 0.4 1</ambient>
+      <background>0.7 0.7 0.7 1</background>
+      <shadows>1</shadows>
+    </scene>
+    <wind/>
+    <spherical_coordinates>
+      <surface_model>EARTH_WGS84</surface_model>
+      <latitude_deg>0</latitude_deg>
+      <longitude_deg>0</longitude_deg>
+      <elevation>0</elevation>
+      <heading_deg>0</heading_deg>
+    </spherical_coordinates>
+    <state world_name='default'>
+      <sim_time>46 800000000</sim_time>
+      <real_time>46 908490360</real_time>
+      <wall_time>1642692487 464939457</wall_time>
+      <iterations>46800</iterations>
+      <model name='ground_plane'>
+        <pose>0 0 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>0 0 0 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <light name='sun'>
+        <pose>0 0 10 0 -0 0</pose>
+      </light>
+    </state>
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose>9.78871 -9.25042 4.66792 0 0.399643 2.3322</pose>
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+      </camera>
+    </gui>
+  </world>
+</sdf>