update

CMakeLists.txt

@@ -49,13 +49,22 @@ target_link_libraries(odom_publisher ${catkin_LIBRARIES})
 
 ## Mark executable scripts (Python etc.) for installation
 ## in contrast to setup.py, you can choose the destination
-catkin_install_python(PROGRAMS
-  nodes/odom_publisher_python.py
-  nodes/odom_publisher_linus.py
-  nodes/odom_publisher_fromTwist.py
+#catkin_install_python(PROGRAMS
+#  nodes/odom_publisher_python.py
+#  nodes/odom_publisher_linus.py
+#  nodes/odom_publisher_fromTwist.py
+#  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+#)
+
+# Find all Python scripts in the nodes/ directory
+file(GLOB python_scripts nodes/*.py)
+
+# Mark all found scripts as executable and install them
+catkin_install_python(PROGRAMS ${python_scripts}
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
+
 ################################################################################
 # Install
 ################################################################################

description/robot_core.xacro

@@ -58,25 +58,35 @@
 
     <!-- BASE LINK -->
 
-    <link name="base_link">
-    </link>
+    <!-- <link name="base_link">
+    </link> -->
 
     <!-- BASE_FOOTPRINT LINK -->
 
-    <joint name="base_footprint_joint" type="fixed">
+    <!-- <joint name="base_footprint_joint" type="fixed">
         <parent link="base_link"/>
         <child link="base_footprint"/>
         <origin xyz="0 0 0" rpy="0 0 0"/>
-    </joint>
+    </joint> -->
 
     <link name="base_footprint">
     </link>
 
+    <!-- BASE LINK -->
+
+    <joint name="base_link_joint" type="fixed">
+        <parent link="base_footprint"/>
+        <child link="base_link"/>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+    </joint>
+
+    <link name="base_link">
+    </link>
 
     <!-- CHASSIS LINK -->
 
     <joint name="chassis_joint" type="fixed">
-        <parent link="base_link"/>
+        <parent link="base_footprint"/>
         <child link="chassis"/>
         <origin xyz="${-chassis_length/2 + 0.075} 0 ${wheel_radius - wheel_offset_z}"/>
     </joint>

launch/robot_pose_ekf.launch

@@ -1,12 +1,18 @@
 <launch>
- <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
+  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
+    
+    <!-- Parameters -->
     <param name="output_frame" value="odom"/>
     <param name="freq" value="30.0"/>
-    <param name="sensor_timeout" value="1.0"/>
+    <param name="sensor_timeout" value="5.0"/>
     <param name="odom_used" value="true"/>
     <param name="imu_used" value="true"/>
     <param name="vo_used" value="false"/>
     <param name="debug" value="false"/>
     <param name="self_diagnose" value="false"/>
+
+    <!-- Remap Odometry Input -->
+    <remap from="/odom" to="/odom_new"/>
+
   </node>
-</launch>
+</launch>

launch/robot_scan_filter.launch

@@ -3,7 +3,6 @@
     <param name="box_filter_config" command="$(find cps_rmp220_support)/config/box_filter.yaml" />
 
     <!-- Launch the laser_filters node -->
-    <node pkg="laser_filters" type="scan_to_scan_filter_chain" name="scan_to_scan_filter_chain" output="screen">
-        <rosparam command="load" file="$(find cps_rmp220_support)/config/box_filter.yaml" />
+    <node pkg="laser_filters" type="scan_to_scan_filter_chain" name="scan_to_scan_filter_chain" output="screen">/>
     </node>
 </launch>

launch/teleop.launch

@@ -0,0 +1,20 @@
+<launch>
+  <!-- Launch the joy node to read input from the Xbox controller -->
+  <node name="joy_node" pkg="joy" type="joy_node" output="screen">
+    <param name="dev" value="/dev/input/js0" />
+    <param name="autorepeat_rate" value="50.0" />
+  </node>
+
+  <!-- Launch the teleop_twist_joy node to map the joystick inputs to robot movement -->
+  <node name="teleop_twist_joy" pkg="teleop_twist_joy" type="teleop_node" output="screen">
+    <!-- Use the Xbox controller configuration -->
+    <param name="config" value="xbox" />
+    <param name="axis_linear" value="1" /> <!-- Left stick up/down for linear velocity -->
+    <param name="axis_angular" value="0" /> <!-- Left stick left/right for angular velocity -->
+    <param name="scale_angular" value="1.0" /> <!-- Scale factor for angular velocity -->
+    <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" />
+  </node>
+</launch>

nodes/encoder_odom_publisher.py

@@ -0,0 +1,133 @@
+#!/usr/bin/env python
+
+import rospy
+import tf
+from nav_msgs.msg import Odometry
+from std_msgs.msg import Header
+from segway_msgs.msg import ticks_fb  # Correct message import
+import math
+
+class EncoderOdometry:
+    def __init__(self):
+        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_base = 0.392  # Distance between wheels (m)
+        self.ticks_per_revolution = 4096  # Encoder ticks per wheel revolution
+
+        # State variables
+        self.x = 0.0
+        self.y = 0.0
+        self.theta = 0.0
+        self.last_l_ticks = None
+        self.last_r_ticks = None
+        self.last_time = None
+
+        # ROS publishers (Updated topic to /odom_new)
+        self.odom_pub = rospy.Publisher("/odom_new", Odometry, queue_size=10)
+        self.odom_broadcaster = tf.TransformBroadcaster()
+
+        # ROS subscriber to /ticks_fb topic
+        rospy.Subscriber("/ticks_fb", ticks_fb, self.ticks_callback)
+
+        rospy.loginfo("Encoder Odometry Publisher Initialized.")
+
+        rospy.spin()
+
+    def ticks_callback(self, msg):
+        rospy.logdebug(f"Received ticks: l_ticks={msg.l_ticks}, r_ticks={msg.r_ticks}, timestamp={msg.ticks_timestamp}")
+
+        if self.last_l_ticks is None or self.last_r_ticks is None or self.last_time is None:
+            rospy.logdebug("Initializing with first set of ticks.")
+            self.last_l_ticks = msg.l_ticks
+            self.last_r_ticks = msg.r_ticks
+            self.last_time = msg.ticks_timestamp / 1e6  # Convert to seconds
+            return
+
+        # Compute time delta
+        current_time = msg.ticks_timestamp / 1e6  # Convert to seconds
+        dt = current_time - self.last_time
+        if dt <= 0:
+            rospy.logwarn(f"Invalid time delta: {dt}. Skipping this message.")
+            return  # Skip if time is invalid
+
+        # Compute tick differences
+        delta_l = msg.l_ticks - self.last_l_ticks
+        delta_r = msg.r_ticks - self.last_r_ticks
+        rospy.logdebug(f"Delta ticks: l={delta_l}, r={delta_r}")
+
+        # Convert ticks to meters
+        meters_per_tick = (2 * math.pi * self.wheel_radius) / self.ticks_per_revolution
+        delta_s_l = delta_l * meters_per_tick
+        delta_s_r = delta_r * meters_per_tick
+        rospy.logdebug(f"Delta displacement: left={delta_s_l} m, right={delta_s_r} m")
+
+        # Compute linear and angular velocity
+        v = (delta_s_l + delta_s_r) / 2.0 / dt
+        omega = (delta_s_r - delta_s_l) / self.wheel_base / dt
+        rospy.logdebug(f"Computed velocity: linear={v} m/s, angular={omega} rad/s")
+
+        # Integrate motion (basic differential drive kinematics)
+        delta_theta = omega * dt
+        self.theta += delta_theta
+        delta_x = v * math.cos(self.theta) * dt
+        delta_y = v * math.sin(self.theta) * dt
+        self.x += delta_x
+        self.y += delta_y
+        rospy.logdebug(f"Integrated position: x={self.x}, y={self.y}, theta={self.theta}")
+
+        # Create odometry message
+        odom = Odometry()
+        odom.header = Header(stamp=rospy.Time.from_sec(current_time), frame_id="odom")
+        odom.child_frame_id = "base_link"
+
+        # Position
+        odom.pose.pose.position.x = self.x
+        odom.pose.pose.position.y = self.y
+        odom.pose.pose.position.z = 0.0
+
+        # Orientation (as quaternion)
+        q = tf.transformations.quaternion_from_euler(0, 0, self.theta)
+        odom.pose.pose.orientation.x = q[0]
+        odom.pose.pose.orientation.y = q[1]
+        odom.pose.pose.orientation.z = q[2]
+        odom.pose.pose.orientation.w = q[3]
+
+        # Velocities
+        odom.twist.twist.linear.x = v
+        odom.twist.twist.angular.z = omega
+
+        # Include Covariance Matrix
+        # Include covariance
+        odom.pose.covariance = [0.01, 0, 0, 0, 0, 0,
+                                           0, 0.01, 0, 0, 0, 0,
+                                           0, 0, 0.01, 0, 0, 0,
+                                           0, 0, 0, 0.01, 0, 0,
+                                           0, 0, 0, 0, 0.01, 0,
+                                           0, 0, 0, 0, 0, 0.01]
+
+        # Publish odometry to /odom_new
+        rospy.logdebug("Publishing Odometry message.")
+        self.odom_pub.publish(odom)
+
+        # # Publish TF transform
+        # rospy.logdebug("Publishing TF transform.")
+        # self.odom_broadcaster.sendTransform(
+        #     (self.x, self.y, 0),
+        #     q,
+        #     rospy.Time.from_sec(current_time),
+        #     "base_link",
+        #     "odom"
+        # )
+
+        # Update previous values
+        self.last_l_ticks = msg.l_ticks
+        self.last_r_ticks = msg.r_ticks
+        self.last_time = current_time
+
+if __name__ == "__main__":
+    try:
+        EncoderOdometry()
+    except rospy.ROSInterruptException:
+        pass

nodes/odom_normalizer.py

@@ -0,0 +1,60 @@
+#!/usr/bin/env python
+
+import rospy
+from nav_msgs.msg import Odometry
+import numpy as np
+
+def normalize_quaternion(qx, qy, qz, qw):
+    norm = np.sqrt(qx**2 + qy**2 + qz**2 + qw**2)
+    if norm == 0:
+        return 0.0, 0.0, 0.0, 1.0  # Default to unit quaternion
+    return qx / norm, qy / norm, qz / norm, qw / norm
+
+def odom_callback(msg):
+    # Normalize quaternion
+    qx, qy, qz, qw = normalize_quaternion(
+        msg.pose.pose.orientation.x,
+        msg.pose.pose.orientation.y,
+        msg.pose.pose.orientation.z,
+        msg.pose.pose.orientation.w
+    )
+
+    # Create new message
+    odom_normalized = Odometry()
+    odom_normalized.header = msg.header
+    odom_normalized.child_frame_id = msg.child_frame_id
+
+    # Copy position
+    odom_normalized.pose.pose.position = msg.pose.pose.position
+
+    # Set normalized quaternion
+    odom_normalized.pose.pose.orientation.x = qx
+    odom_normalized.pose.pose.orientation.y = qy
+    odom_normalized.pose.pose.orientation.z = qz
+    odom_normalized.pose.pose.orientation.w = qw
+
+    # Copy covariance
+    odom_normalized.pose.covariance = msg.pose.covariance
+    odom_normalized.twist = msg.twist  # Copy twist data
+
+    # Include covariance
+    odom_normalized.pose.covariance = [0.01, 0, 0, 0, 0, 0,
+                                       0, 0.01, 0, 0, 0, 0,
+                                       0, 0, 0.01, 0, 0, 0,
+                                       0, 0, 0, 0.01, 0, 0,
+                                       0, 0, 0, 0, 0.01, 0,
+                                       0, 0, 0, 0, 0, 0.01]
+
+    # Publish normalized odom
+    pub.publish(odom_normalized)
+
+if __name__ == "__main__":
+    rospy.init_node("odom_normalizer")
+
+    # Publisher
+    pub = rospy.Publisher("/odom_normalized", Odometry, queue_size=10)
+
+    # Subscriber
+    rospy.Subscriber("/odom", Odometry, odom_callback)
+
+    rospy.spin()