config/twist_mux_topics.yaml

@@ -10,19 +10,19 @@ topics:
   name    : navigation
   topic   : nav_vel
   timeout : 0.5
-  priority: 110
+  priority: 100
 -
   name    : joystick
   topic   : joy_vel
   timeout : 0.5
-  priority: 100
+  priority: 90
 -
   name    : keyboard
   topic   : key_vel
   timeout : 0.5
-  priority: 10
+  priority: 90
 -
   name    : tablet
   topic   : tab_vel
   timeout : 0.5
-  priority: 10
+  priority: 90

launch/joystick.yaml

@@ -1,10 +0,0 @@
-priority: True
-
-turbo:
-  linear_forward_min  : 0.5
-  linear_forward_max  : 1.0
-  linear_backward_min : 0.25
-  linear_backward_max : 0.5
-  angular_min : 0.7
-  angular_max : 1.2
-  steps       : 4

launch/linus_amcl.launch

@@ -48,12 +48,5 @@
     <param name="global_frame_id" value="map"/>
     <param name="tf_broadcast" value="true" />  -->
 
-    <!-- scan topic -->
-    <!-- <remap from="scan" to="scan_filtered"/> -->
-    <!-- remap cmd_vel -->
-    <remap from="cmd_vel" to="nav_vel"/>
-    <!-- remap odom -->
-    <remap from="odom" to="/robot_pose_ekf/odom_combined"/>
-
   </node>
 </launch>

launch/move_base_core.launch

@@ -1,30 +0,0 @@
-<launch>
-  <param name="use_sim_time" value="false" />
-
-  <!--<include file="$(find segwayrmp)/launch/segwayrmp_configuration.launch" />-->
-
-  <!-- Run the map server -->
-  <!-- <node name="map_server" pkg="map_server" type="map_server" args="$(find segwayrmp)/maps/mymap.pgm my_map_resolution"/> -->
-  <!-- <node name="map_server" pkg="map_server" type="map_server" args="$(find cps_rmp220_support)/maps/map.yaml"/> -->
-
-  <!--move base-->
-  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
-    <rosparam file="$(find segwayrmp)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
-    <rosparam file="$(find segwayrmp)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
-    <rosparam file="$(find segwayrmp)/param/local_costmap_params.yaml" command="load" />
-    <rosparam file="$(find segwayrmp)/param/global_costmap_params.yaml" command="load" />
-    <rosparam file="$(find segwayrmp)/param/base_local_planner_params.yaml" command="load" />
-    <!-- scan topic -->
-    <remap from="scan" to="scan_filtered"/>
-    <!-- remap cmd_vel -->
-    <remap from="cmd_vel" to="nav_vel"/>
-    <!-- remap odom -->
-    <remap from="odom" to="/robot_pose_ekf/odom_combined"/>
-  </node>
-
-  <!-- TF -->
-  <!-- <node pkg="tf" type="static_transform_publisher" name="base_to_laser" args="0.10 0.0 0.12 3.14 0.0 0.0 base_link laser 100"/>
-  static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms -->  
-  <!-- <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_base_link" args="0 0 0.5 0 0 0 base_link base_footprint 50" /> -->
-
-</launch>

launch/teleop.launch

@@ -15,7 +15,6 @@
     <param name="scale_linear" value="0.5" /> <!-- Scale factor for linear velocity -->
     
     <!-- Remap /cmd_vel to /joy_vel -->
-    <!-- <remap from="/cmd_vel" to="/joy_vel" /> -->
+    <remap from="/cmd_vel" to="/joy_vel" />
-    <remap from="/cmd_vel" to="/input_joy/cmd_vel" />
   </node>
 </launch>

launch/twist_mux.launch

@@ -5,9 +5,9 @@
   <arg name="cmd_vel_out" default="twist_mux/cmd_vel"/>
 
   <arg name="config_locks"  default="$(find twist_mux)/config/twist_mux_locks.yaml"/>
-  <arg name="config_topics" default="$(find cps_rmp220_support)/config/twist_mux_topics.yaml"/>
+  <arg name="config_topics" default="$(find cps_rmp220_Support)/config/twist_mux_topics.yaml"/>
 
-  <arg name="config_joy" default="$(find cps_rmp220_support)/config/joystick.yaml"/>
+  <arg name="config_joy" default="$(find twist_mux)/config/joystick.yaml"/>
 
   <node pkg="twist_mux" type="twist_mux" name="twist_mux" output="screen">
     <remap from="cmd_vel_out" to="$(arg cmd_vel_out)"/>

nodes/encoder_odom_publisher.py

@@ -1,9 +1,5 @@
 #!/usr/bin/env python
 
-""" 
-Node for pusblishing segway rmp 220 lite Odometry calculated from encoder ticks_fb
-"""
-
 import rospy
 import tf
 from nav_msgs.msg import Odometry
@@ -16,9 +12,9 @@ class EncoderOdometry:
         rospy.init_node("encoder_odom_publisher")
 
         # Robot parameters (update these according to your robot)
-        self.wheel_radius = 0.125 #0.202  # Wheel radius in meters
+        self.wheel_radius = 0.202  # Wheel radius in meters
-        self.wheel_base = 0.415 #0.392  # Distance between wheels (m)
+        self.wheel_base = 0.392  # Distance between wheels (m)
-        self.ticks_per_revolution = 4096  # Encoder ticks per wheel revolution --> checked fine
+        self.ticks_per_revolution = 4096  # Encoder ticks per wheel revolution
 
         # State variables
         self.x = 0.0