From cc4cdacb28644117acf76177034a2b468c39501b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bj=C3=B6rn=20Ellensohn?= <bjoern.ellensohn@gmail.com>
Date: Mon, 31 Jul 2023 09:51:32 +0200
Subject: [PATCH] refactoring urdfs, update launchers

---
 description/camera.xacro                      |  2 +-
 description/robot_core.xacro                  |  8 +--
 ...unch.py => !dont_use_launch_sim.launch.py} |  0
 launch/joystick.launch.py                     | 35 -----------
 launch/robot_mapping.launch.py                | 62 +++++++++++++++++++
 5 files changed, 66 insertions(+), 41 deletions(-)
 rename launch/{launch_sim.launch.py => !dont_use_launch_sim.launch.py} (100%)
 delete mode 100644 launch/joystick.launch.py
 create mode 100644 launch/robot_mapping.launch.py

diff --git a/description/camera.xacro b/description/camera.xacro
index 0fbcae9..189ddf1 100644
--- a/description/camera.xacro
+++ b/description/camera.xacro
@@ -4,7 +4,7 @@
     <joint name="camera_joint" type="fixed">
         <parent link="chassis"/>
         <child link="camera_link"/>
-        <origin xyz="0.276 0 0.181" rpy="0 0.18 0"/>
+        <origin xyz="0.276 0 0.181" rpy="0 0 0"/>
     </joint>
 
     <link name="camera_link">
diff --git a/description/robot_core.xacro b/description/robot_core.xacro
index e1749f4..884739d 100644
--- a/description/robot_core.xacro
+++ b/description/robot_core.xacro
@@ -110,7 +110,7 @@
     <joint name="left_wheel_joint" type="continuous">
         <parent link="base_link"/>
         <child link="left_wheel"/>
-        <origin xyz="0 ${wheel_offset_y} 0" rpy="-${pi/2} 0 0" />
+        <origin xyz="${chassis_length/2 - 0.070} ${wheel_offset_y/2 + wheel_thickness/2 + 0.01} 0" rpy="-${pi/2} 0 0" />
         <axis xyz="0 0 1"/>
     </joint>
 
@@ -143,7 +143,7 @@
     <joint name="right_wheel_joint" type="continuous">
         <parent link="base_link"/>
         <child link="right_wheel"/>
-        <origin xyz="0 ${-wheel_offset_y} 0" rpy="${pi/2} 0 0" />
+        <origin xyz="${chassis_length/2 - 0.070} -${wheel_offset_y/2 + wheel_thickness/2 + 0.01} 0" rpy="${pi/2} 0 0" />
         <axis xyz="0 0 -1"/>
     </joint>
 
@@ -206,7 +206,7 @@
     <joint name="lidar_joint" type="fixed">
         <parent link="chassis"/>
         <child link="laser_frame"/>
-        <origin xyz="-150 0 30"/>
+        <origin xyz="${chassis_length -0.150} 0 ${chassis_height + 0.030}" rpy="0 0 0"/>
     </joint>
 
 
@@ -229,8 +229,6 @@
 
     <gazebo reference="laser_frame">
         <material>Gazebo/White</material>
-        <mu1 value="0.001"/>
-        <mu2 value="0.001"/>
     </gazebo>
 
 </robot>
\ No newline at end of file
diff --git a/launch/launch_sim.launch.py b/launch/!dont_use_launch_sim.launch.py
similarity index 100%
rename from launch/launch_sim.launch.py
rename to launch/!dont_use_launch_sim.launch.py
diff --git a/launch/joystick.launch.py b/launch/joystick.launch.py
deleted file mode 100644
index 4c707b3..0000000
--- a/launch/joystick.launch.py
+++ /dev/null
@@ -1,35 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-from launch.substitutions import LaunchConfiguration
-from launch.actions import DeclareLaunchArgument
-
-import os
-from ament_index_python.packages import get_package_share_directory
-
-def generate_launch_description():
-    use_sim_time = LaunchConfiguration('use_sim_time')
-
-    joy_params = os.path.join(get_package_share_directory('segway_rmp220_support'),'config','joystick.yaml')
-
-    joy_node = Node(
-            package='joy',
-            executable='joy_node',
-            parameters=[joy_params, {'use_sim_time': use_sim_time}],
-         )
-
-    teleop_node = Node(
-            package='teleop_twist_joy',
-            executable='teleop_node',
-            name='teleop_node',
-            parameters=[joy_params, {'use_sim_time': use_sim_time}],
-            remappings=[('/cmd_vel','/cmd_vel_joy')]
-         )
-    
-    return LaunchDescription([
-        DeclareLaunchArgument(
-            'use_sim_time',
-            default_value='false',
-            description='Use sim time if true'),
-        joy_node,
-        teleop_node,
-    ])
\ No newline at end of file
diff --git a/launch/robot_mapping.launch.py b/launch/robot_mapping.launch.py
new file mode 100644
index 0000000..d2b9260
--- /dev/null
+++ b/launch/robot_mapping.launch.py
@@ -0,0 +1,62 @@
+# Copyright 2022 Factor Robotics
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+from launch import LaunchDescription
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from ament_index_python.packages import get_package_share_directory
+from launch_ros.actions import LifecycleNode
+from launch_ros.descriptions import ParameterValue
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("cps_rmp220_support"),
+                    "description",
+                    "robot.urdf.xacro"
+                ]
+            ),
+        ]
+    )
+
+    use_sim_time = False
+    slam_params_file = PathJoinSubstitution(
+                [
+                    FindPackageShare("bot_mini_bringup"),
+                    "config",
+                    "mapper_params_online_async.yaml"
+                ]
+            )
+    mapper_node = Node(
+        package="slam_toolbox",
+        executable="async_slam_toolbox_node",
+        name='slam_toolbox_node',
+        output='screen',
+        parameters=[
+            slam_params_file,
+            {'use_sim_time': use_sim_time}
+        ],
+    )
+
+    return LaunchDescription([
+        mapper_node
+    ])
\ No newline at end of file