From 54dbf0a7aa6bd3c74ae102b58c32f334ada952b3 Mon Sep 17 00:00:00 2001
From: SimonR99 <simonroy99@hotmail.ca>
Date: Thu, 14 Mar 2024 19:11:50 -0400
Subject: [PATCH] fix teleop path

---
 .../rove_controller_bluethoot.launch.py       |   2 +-
 .../launch/rove_controller_usb.launch.py      |   2 +-
 src/rove_description/config/robot_bridge.yaml |  16 ---
 src/rove_description/launch/sim.launch.py     |   8 ++
 src/rove_description/urdf/command.urdf.xacro  | 101 ++++++++++++++++++
 src/rove_description/urdf/flipper.urdf.xacro  |   6 +-
 src/rove_description/urdf/gazebo.urdf.xacro   |  18 ++--
 src/rove_description/urdf/rove.urdf.xacro     |   2 +-
 src/rove_description/urdf/track.urdf.xacro    |   4 +-
 src/rove_slam/config/ekf.yaml                 |   2 +-
 10 files changed, 127 insertions(+), 34 deletions(-)
 delete mode 100644 src/rove_description/config/robot_bridge.yaml
 create mode 100644 src/rove_description/urdf/command.urdf.xacro

diff --git a/src/rove_bringup/launch/rove_controller_bluethoot.launch.py b/src/rove_bringup/launch/rove_controller_bluethoot.launch.py
index a52ec66..efec645 100644
--- a/src/rove_bringup/launch/rove_controller_bluethoot.launch.py
+++ b/src/rove_bringup/launch/rove_controller_bluethoot.launch.py
@@ -24,7 +24,7 @@ def generate_launch_description():
             name='teleop_twist_joy_node',
             parameters=[teleope_joy_params_file],
             remappings=[
-                ('/joy', '/rove/joy'),
+                ('/joy', '/joy'),
                 # ('/cmd_vel', '/model/rove/cmd_vel')
             ],
         ),
diff --git a/src/rove_bringup/launch/rove_controller_usb.launch.py b/src/rove_bringup/launch/rove_controller_usb.launch.py
index 82ea85a..846e720 100644
--- a/src/rove_bringup/launch/rove_controller_usb.launch.py
+++ b/src/rove_bringup/launch/rove_controller_usb.launch.py
@@ -24,7 +24,7 @@ def generate_launch_description():
             name='teleop_twist_joy_node',
             parameters=[teleope_joy_params_file],
             remappings=[
-                ('/joy', '/rove/joy'),
+                ('/joy', '/joy'),
                 # ('/cmd_vel', '/model/rove/cmd_vel')
             ],
         ),
diff --git a/src/rove_description/config/robot_bridge.yaml b/src/rove_description/config/robot_bridge.yaml
deleted file mode 100644
index 040dd4b..0000000
--- a/src/rove_description/config/robot_bridge.yaml
+++ /dev/null
@@ -1,16 +0,0 @@
----
-- ros_topic_name: "/rove/cmd_vel"
-  gz_topic_name: "/model/rove/cmd_vel"
-  ros_type_name: "geometry_msgs/msg/Twist"
-  gz_type_name: "gz.msgs.Twist"
-  direction: ROS_TO_GZ
-- ros_topic_name: "/rove/odom"
-  gz_topic_name: "/model/rove/odometry"
-  ros_type_name: "nav_msgs/msg/Odometry"
-  gz_type_name: "gz.msgs.Odometry"
-  direction: GZ_TO_ROS
-- ros_topic_name: "/rove/scan"
-  gz_topic_name: "/model/rove/lidar"
-  ros_type_name: "sensor_msgs/msg/LaserScan"
-  gz_type_name: "ignition.msgs.LaserScan"
-  direction: GZ_TO_ROS
\ No newline at end of file
diff --git a/src/rove_description/launch/sim.launch.py b/src/rove_description/launch/sim.launch.py
index 01516fa..64bb242 100644
--- a/src/rove_description/launch/sim.launch.py
+++ b/src/rove_description/launch/sim.launch.py
@@ -20,6 +20,7 @@ def generate_launch_description():
     pkg_rove_nav = get_package_share_directory('rove_navigation')
     slam_pkg_path = get_package_share_directory("slam_toolbox")
     pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
+    bringup_pkg_path = get_package_share_directory('rove_bringup')
 
     # Get the URDF file
     urdf_path = os.path.join(pkg_rove_description, 'urdf', 'rove.urdf.xacro')
@@ -108,6 +109,12 @@ def generate_launch_description():
                    {'use_sim_time': True}]
                    )
 
+    teleop = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(bringup_pkg_path, "launch", "rove_controller_usb.launch.py"),
+        ),
+    )
+
     return LaunchDescription([
             gz_sim,
             DeclareLaunchArgument('rviz', default_value='true',
@@ -119,4 +126,5 @@ def generate_launch_description():
             slam,
             create,
             nav,
+            teleop,
             ])
diff --git a/src/rove_description/urdf/command.urdf.xacro b/src/rove_description/urdf/command.urdf.xacro
new file mode 100644
index 0000000..c265b82
--- /dev/null
+++ b/src/rove_description/urdf/command.urdf.xacro
@@ -0,0 +1,101 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <plugin filename="gz-sim-triggered-publisher-system"
+        name="gz::sim::systems::TriggeredPublisher">
+        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
+            <match field="data">87</match>
+        </input>
+        <output type="gz.msgs.Twist" topic="cmd_vel">
+            linear: {x: 1.0}, angular: {z: 0.0}
+        </output>
+    </plugin>
+
+    <!-- Moving Backward: X -->
+    <plugin filename="gz-sim-triggered-publisher-system"
+        name="gz::sim::systems::TriggeredPublisher">
+        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
+            <match field="data">88</match>
+        </input>
+        <output type="gz.msgs.Twist" topic="cmd_vel">
+            linear: {x: -1.0}, angular: {z: 0.0}
+        </output>
+    </plugin>
+
+    <!-- Stop: S -->
+    <plugin filename="gz-sim-triggered-publisher-system"
+        name="gz::sim::systems::TriggeredPublisher">
+        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
+            <match field="data">83</match>
+        </input>
+        <output type="gz.msgs.Twist" topic="cmd_vel">
+            linear: {x: 0.0}, angular: {z: 0.0}
+        </output>
+    </plugin>
+
+    <!-- Left: A -->
+    <plugin filename="gz-sim-triggered-publisher-system"
+        name="gz::sim::systems::TriggeredPublisher">
+        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
+            <match field="data">65</match>
+        </input>
+        <output type="gz.msgs.Twist" topic="cmd_vel">
+            linear: {x: 0.0}, angular: {z: 1.0}
+        </output>
+    </plugin>
+
+    <!-- Right: D -->
+    <plugin filename="gz-sim-triggered-publisher-system"
+        name="gz::sim::systems::TriggeredPublisher">
+        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
+            <match field="data">68</match>
+        </input>
+        <output type="gz.msgs.Twist" topic="cmd_vel">
+            linear: {x: 0.0}, angular: {z: -1.0}
+        </output>
+    </plugin>
+
+    <!-- Forward Left: Q -->
+    <plugin filename="gz-sim-triggered-publisher-system"
+        name="gz::sim::systems::TriggeredPublisher">
+        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
+            <match field="data">81</match>
+        </input>
+        <output type="gz.msgs.Twist" topic="cmd_vel">
+            linear: {x: 1.0}, angular: {z: 1.0}
+        </output>
+    </plugin>
+
+    <!-- Forward Right: E -->
+    <plugin filename="gz-sim-triggered-publisher-system"
+        name="gz::sim::systems::TriggeredPublisher">
+        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
+            <match field="data">69</match>
+        </input>
+        <output type="gz.msgs.Twist" topic="cmd_vel">
+            linear: {x: 1.0}, angular: {z: -1.0}
+        </output>
+    </plugin>
+
+    <!-- Back Left: Z -->
+    <plugin filename="gz-sim-triggered-publisher-system"
+        name="gz::sim::systems::TriggeredPublisher">
+        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
+            <match field="data">90</match>
+        </input>
+        <output type="gz.msgs.Twist" topic="cmd_vel">
+            linear: {x: -1.0}, angular: {z: 1.0}
+        </output>
+    </plugin>
+
+    <!-- Back Right: C -->
+    <plugin filename="gz-sim-triggered-publisher-system"
+        name="gz::sim::systems::TriggeredPublisher">
+        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
+            <match field="data">67</match>
+        </input>
+        <output type="gz.msgs.Twist" topic="cmd_vel">
+            linear: {x: -1.0}, angular: {z: -1.0}
+        </output>
+    </plugin>
+
+</robot>
\ No newline at end of file
diff --git a/src/rove_description/urdf/flipper.urdf.xacro b/src/rove_description/urdf/flipper.urdf.xacro
index 9033fd6..4f4520a 100644
--- a/src/rove_description/urdf/flipper.urdf.xacro
+++ b/src/rove_description/urdf/flipper.urdf.xacro
@@ -36,7 +36,7 @@
             <dynamics damping="50.0" friction="50"/>
         </joint>
 
-       
+
 
 
         <link name="flipper_${suffix}">
@@ -54,7 +54,7 @@
                     <mesh filename="file://$(find rove_description)/meshes/flipper.stl"/>
                 </geometry>
                 <surface>
-                    <friction>
+<friction>
                         <ode>
                             <mu>0.65</mu>
                             <mu2>0.85</mu2>
@@ -75,7 +75,7 @@
             </inertial>
         </link>
         <gazebo>
-            <plugin name='ignition::gazebo::systems::TrackController' filename='libignition-gazebo-track-controller-system.so'>
+            <plugin name='gz::sim::systems::TrackController' filename='gz-sim-track-controller-system'>
                     <link>flipper_${suffix}</link>
                     <min_velocity>-3.0</min_velocity>
                     <max_velocity>3.0</max_velocity>
diff --git a/src/rove_description/urdf/gazebo.urdf.xacro b/src/rove_description/urdf/gazebo.urdf.xacro
index 710495f..f79f405 100644
--- a/src/rove_description/urdf/gazebo.urdf.xacro
+++ b/src/rove_description/urdf/gazebo.urdf.xacro
@@ -3,14 +3,14 @@
 
     <gazebo>
         <plugin
-            filename="ignition-gazebo-joint-state-publisher-system"
-            name="ignition::gazebo::systems::JointStatePublisher">
+            filename="gz-sim-joint-state-publisher-system"
+            name="gz::sim::systems::JointStatePublisher">
             <topic>joint_states</topic>
         </plugin>
 
         <plugin
-            filename="ignition-gazebo-imu-system"
-            name="ignition::gazebo::systems::Imu">
+            filename="gz-sim-imu-system"
+            name="gz::sim::systems::Imu">
         </plugin>
 
 
@@ -22,12 +22,12 @@
             <right_track><link>flipper_fr</link></right_track>
             <right_track><link>flipper_rr</link></right_track>
 
-             <!-- TODO PUT ACTUAL VALUES -->
+            <!-- TODO PUT ACTUAL VALUES -->
             <tracks_separation>0.63</tracks_separation>
             <tracks_height>0.18094</tracks_height>
             <steering_efficiency>1</steering_efficiency>
 
-             <topic>cmd_vel</topic>
+            <topic>cmd_vel</topic>
 
             <odom_topic>odom</odom_topic>
             <frame_id>odom</frame_id>
@@ -35,14 +35,14 @@
             <odom_publisher_frequency>30</odom_publisher_frequency>
             
             <publish_odom_tf>false</publish_odom_tf>
-            <publish_wheel_tf>true</publish_wheel_tf>
+            <publish_wheel_tf>false</publish_wheel_tf>
 
             <tf_topic>/tf</tf_topic>
         </plugin>
 
         <plugin 
-            filename="ignition-gazebo-sensors-system"
-            name="ignition::gazebo::systems::Sensors">
+            filename="gz-sim-sensors-system"
+            name="gz::sim::systems::Sensors">
             <render_engine>ogre2</render_engine>
         </plugin>
     </gazebo>
diff --git a/src/rove_description/urdf/rove.urdf.xacro b/src/rove_description/urdf/rove.urdf.xacro
index 246d5e2..ed461d0 100644
--- a/src/rove_description/urdf/rove.urdf.xacro
+++ b/src/rove_description/urdf/rove.urdf.xacro
@@ -13,7 +13,7 @@
     <xacro:include filename="$(find rove_description)/urdf/gazebo.urdf.xacro" />
     <xacro:include filename="$(find rove_description)/urdf/lidar.urdf.xacro" />
     <xacro:include filename="$(find rove_description)/urdf/imu.urdf.xacro" />
-
+    
     <xacro:include filename="$(find rove_description)/urdf/flipper.urdf.xacro" />
     <xacro:include filename="$(find rove_description)/urdf/track.urdf.xacro" />
     
diff --git a/src/rove_description/urdf/track.urdf.xacro b/src/rove_description/urdf/track.urdf.xacro
index 9d0a564..bb8c100 100644
--- a/src/rove_description/urdf/track.urdf.xacro
+++ b/src/rove_description/urdf/track.urdf.xacro
@@ -55,8 +55,8 @@
         <gazebo>
             <plugin name='gz::sim::systems::TrackController' filename='libignition-gazebo-track-controller-system.so'>
                     <link>track_${suffix}</link>
-                    <min_velocity>-1.0</min_velocity>
-                    <max_velocity>1.0</max_velocity>
+                    <min_velocity>-3.0</min_velocity>
+                    <max_velocity>3.0</max_velocity>
                     <!--debug>1</debug-->
             </plugin>
         </gazebo>
diff --git a/src/rove_slam/config/ekf.yaml b/src/rove_slam/config/ekf.yaml
index 9805e19..1f406cf 100644
--- a/src/rove_slam/config/ekf.yaml
+++ b/src/rove_slam/config/ekf.yaml
@@ -13,7 +13,7 @@ ekf_filter_node:
 
         odom0: /odom
         odom0_config: [true,  true,  true,
-                       false, false, false,
+                       false, false, true,
                        false, false, false,
                        false, false, true,
                        false, false, false]