update

config/twist_mux_topics.yaml

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

launch/joystick.yaml

@@ -0,0 +1,10 @@
+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,5 +48,12 @@
     <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

@@ -0,0 +1,30 @@
+<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,6 +15,7 @@
     <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 twist_mux)/config/joystick.yaml"/>
+  <arg name="config_joy" default="$(find cps_rmp220_support)/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,5 +1,9 @@
 #!/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
@@ -12,9 +16,9 @@ class EncoderOdometry:
         rospy.init_node("encoder_odom_publisher")
 
         # Robot parameters (update these according to your robot)
-        self.wheel_radius = 0.202  # Wheel radius in meters
+        self.wheel_radius = 0.125 #0.202  # Wheel radius in meters
-        self.wheel_base = 0.392  # Distance between wheels (m)
+        self.wheel_base = 0.415 #0.392  # Distance between wheels (m)
-        self.ticks_per_revolution = 4096  # Encoder ticks per wheel revolution
+        self.ticks_per_revolution = 4096  # Encoder ticks per wheel revolution --> checked fine
 
         # State variables
         self.x = 0.0