Subscriber doesn't receive any messages

Question

I am currently simulating a mecanum wheel robot in Gazebo. My goal is to control the individual wheel velocities based on a given target point. The issue I'm facing is that although I can control the velocities using the target point, the odometry subscriber I created is not updating the robot's coordinates in real-time, which is causing problems in the simulation.

Below are three files, including a Python file for controlling the movement of the car,a URDF file for the car, and controller.yaml

Python file for controlling the movement of the car

#! /usr/bin/env python

import traceback
import rospy
from geometry_msgs.msg import Twist, Point
from nav_msgs.msg import Odometry
from std_msgs.msg import Float64
from sensor_msgs.msg import LaserScan
import numpy as np
import math
from tf import transformations
from std_srvs.srv import *

position_ = Point(x=0.0, y=2.0, z=1.0)
yaw_ = 0
#goal
desired_position_ = Point()
desired_position_.x = 5
desired_position_.y = -8
desired_position_.z = 0

state_= 0
active_ = False
# parameters
yaw_precision_ = math.pi / 90 # +/- 2 degree allowed
dist_precision_ = 0.3


width = {"fr": 0.275, "fl": 0.275, "rr": 0.275, "rl": 0.275}
length = {"fr": 0.575, "fl": 0.575, "rr": 0.575, "rl": 0.575}


fr_pub = None
fl_pub = None
rr_pub = None
rl_pub = None
pub = None
vf = 0
vb = 0
vl = 0
vr = 0


mat = np.matrix([[1, 1, (width["fr"] + length["fr"])],
                 [1, -1, -(width["fl"] + length["fl"])],
                 [1, -1, (width["rr"] + length["rr"])],
                 [1, 1, -(width["rl"] + length["rl"])]])
                 

cmd_vel = np.matrix([0, 0, 0])

def cmdVelCB(data):
    global cmd_vel
    cmd_vel = np.matrix([data.linear.x, data.linear.y, data.angular.z])
    
def normalize_angle(angle):
    if (math.fabs(angle) > math.pi):
        angle = angle -(2 * math.pi * angle) / (math.fabs(angle))
    return angle
    
def change_state(state):
    global state_
    state_ = state
    print ('State changed to [%s]' % state_)

def go_to_point_switch(req):
    global active_
    active_ = req.data
    res = SetBoolResponse()
    res.success = True
    res.message = 'Done'
    return res

def go_to_point_omni(des_pos): # fixed the angle between car and goal
    global yaw_, cmd_vel, yaw_precision_, state_, fr,fl, rl, rr, vb, vf, vl, vr, pub
    print("vf:", vf)
    print("vb:", vb)
    print("vl:", vl)
    print("vr:", vr)

    err_yaw = normalize_angle(des_pos.z - yaw_)
    err_pos_y = des_pos.y - position_.y
    err_pos_x = des_pos.x - position_.x
    robot_err_x = np.cos(yaw_) * err_pos_x + np.sin(yaw_) * err_pos_y
    robot_err_y = -np.sin(yaw_) * err_pos_x + np.cos(yaw_) * err_pos_y
    print("des_pos.x:", des_pos.x)
    print("des_pos.y:", des_pos.y)
    print("position_.x:", position_.x)
    print("position_.y", position_.x)
    print("des_pos.z:", des_pos.z)
    print("yaw_:", yaw_)

    print("err_yaw:", err_yaw)
    print("err_pos_x:", err_pos_x)
    print("err_pos_y:", err_pos_y)
    print("robot_err_x", robot_err_x)
    print("robot_err_y:", robot_err_y)

    twist_msg = Twist()
    twist_msg.angular.z = 0
    print("x,y,z:", twist_msg.linear.x,twist_msg.linear.y,twist_msg.angular.z)
    there_is_error = False
    if math.fabs(robot_err_y) > dist_precision_:
        if robot_err_y > 0:
            twist_msg.linear.y = 5 + vr
        else:
            twist_msg.linear.y = -5 + vl
        there_is_error = True
        #twist_msg.angular.z = -0.3 if err_yaw > 0 else 0.3
    if math.fabs(robot_err_x) > dist_precision_:
        if robot_err_x > 0:
            twist_msg.linear.x = 5 + vf
        else:
            twist_msg.linear.x = -5 + vb
        there_is_error = True 

    cmd_vel = np.matrix([twist_msg.linear.x, twist_msg.linear.y, twist_msg.angular.z])
    print("cmd_vel", cmd_vel)
    wheel_vel = (np.dot(mat, cmd_vel.T).A1).tolist()

    wv = Float64()
    wv.data = wheel_vel[0]
    fr_pub.publish(wv)
    wv.data = wheel_vel[1]
    fl_pub.publish(wv)
    wv.data = wheel_vel[2]
    rr_pub.publish(wv)
    wv.data = wheel_vel[3]
    rl_pub.publish(wv)
    pub.publish(twist_msg)
    print("there_is_error: ", there_is_error)

    if not there_is_error :
        print ('Arrivaled')
        change_state(1)

def LidarCallback(msg):
    global vf, vb, vl, vr
    dist_front = msg.ranges[180]
    dist_right = msg.ranges[90]
    dist_left = msg.ranges[270]
    dist_back = msg.ranges[0]
    safe_distance = 0.6


    if dist_front < safe_distance:
        vf = -0.5  
    else:
        vf = 0.5

    if dist_right < safe_distance:
        vr = -0.5  
    else:
        vr = 0.5

    if dist_left < safe_distance:
        vl = -0.5  
    else:
        vl = 0.5
    if dist_back < safe_distance:
        vb = -0.5 
    else: 
        vb = 0.5


def done():
    global cmd_vel,fr,fl, rl, rr, pub
    twist_msg = Twist()
    twist_msg.linear.x = 0
    twist_msg.linear.y = 0
    twist_msg.angular.z = 0
    cmd_vel = np.matrix([twist_msg.linear.x , twist_msg.linear.y, twist_msg.angular.z])
    # pub.publish(twist_msg)
    wheel_vel = (np.dot(mat, cmd_vel.T).A1).tolist()
    wv = Float64()
    wv.data = wheel_vel[0]
    fr_pub.publish(wv)
    wv.data = wheel_vel[1]
    fl_pub.publish(wv)
    wv.data = wheel_vel[2]
    rr_pub.publish(wv)
    wv.data = wheel_vel[3]
    rl_pub.publish(wv)

def clbk_odom(msg):
    global position_
    global yaw_
    
    # position
    position_ = msg.pose.pose.position
    # yaw
    quaternion = (
        msg.pose.pose.orientation.x,
        msg.pose.pose.orientation.y,
        msg.pose.pose.orientation.z,
        msg.pose.pose.orientation.w)
    euler = transformations.euler_from_quaternion(quaternion)
    yaw_ = euler[2]
    rospy.loginfo("Received PoseStamped message")
    rospy.loginfo("Position: x=%f, y=%f, z=%f", position_.x, position_.y, position_.z)
    rospy.loginfo("Orientation: x=%f, y=%f, z=%f, w=%f", msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w)

def odom_callback(msg):
    print("here")
    rospy.loginfo("Received odometry message:")
    rospy.loginfo("Header: %s", msg.header)
    rospy.loginfo("Child frame ID: %s", msg.child_frame_id)
    rospy.loginfo("Pose: %s", msg.pose)
    rospy.loginfo("Twist: %s", msg.twist)

def process():
    global fr_pub, fl_pub, rr_pub, rl_pub, state_, active_ ,pub

    rospy.init_node('test_mecanum_robot', anonymous=False)
    # loop_rate = rospy.Rate(10)
    #for velocity
    fr_pub = rospy.Publisher('/front_right_controller/command', Float64, queue_size=10)
    fl_pub = rospy.Publisher('/front_left_controller/command', Float64, queue_size=10)
    rr_pub = rospy.Publisher('/rear_right_controller/command', Float64, queue_size=10)
    rl_pub = rospy.Publisher('/rear_left_controller/command', Float64, queue_size=10)
    srv = rospy.Service('/switch', SetBool, go_to_point_switch)
    rospy.Subscriber('/cmd_vel', Twist, cmdVelCB, queue_size=10)
    rospy.Subscriber('/agv/laser/scan', LaserScan, LidarCallback, queue_size=10)
    #for position
    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
    rospy.Subscriber('/odom', Odometry, odom_callback, queue_size=10)
    rate = rospy.Rate(20)
    while not rospy.is_shutdown():
        rospy.loginfo("Before checking active_")
        if not active_:
            rospy.loginfo("active_ is False")
            continue
        else:
            rospy.loginfo("After checking active_")
            if state_ == 0:
                rospy.loginfo("State_ 0")
                go_to_point_omni(desired_position_)
            elif state_ == 1: #arrival
                rospy.loginfo("State_ 1")
                done()
                pass
            else:
                rospy.loginfo("bye")
                rospy.logerr('Unknown state!')
                pass
        rate.sleep()

if __name__ == '__main__':
    try:
        process()
    except Exception as ex:
        print(traceback.print_exc())

URDF file for the car

<?xml version="1.0" ?>
<robot name="mecanum_wheel_robot" xmlns:xacro="http://ros.org/wiki/xacro">

 <xacro:include filename="$(find ros_car)/urdf/mecanum_wheel_macro.xacro" />
 <xacro:include filename="$(find ros_car)/urdf/sensor.gazebo.xacro" />
  
 <link name="dummy"/>
 <joint name="dummy_joint" type="fixed">
    <parent link="dummy"/>
    <child link="base_link"/>
 </joint>
  
  <link name="base_link">
    <visual>
      <geometry>
        <box size="0.7 0.4 0.3" />
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="0.7 0.4 0.3" />
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0" />
      <mass value="10.0" />
      <inertia ixx="0.83333" ixy="0.0" ixz="0.0"
              iyy="1.9333" iyz="0.0"
              izz="2.1667" />
    </inertial>
    
  </link>


  <link name="hokuyo">
     <visual>
        <origin xyz="0.0 0 0" rpy=" 0 0 0"/>
        <geometry>
          <mesh filename="package://ros_car/mesh/rplidar.dae"/>
        </geometry>
     </visual>
     <collision>
        <origin xyz="0.0 0 0" rpy=" 0 0 0"/>
        <geometry>
          <box size="0.2 0.2 0.2"/>
        </geometry>
     </collision>
     <inertial>
        <mass value="0.1"/>
        <origin xyz="0.0 0 0" rpy=" 0 0 0"/>
        <inertia
            ixx="1e-6" ixy="0" ixz="0"
            iyy="1e-6" iyz="0"
            izz="1e-6"
        />
     </inertial>
  </link>
  
  <gazebo reference="base_link">
    <mu1 value="0.6"/>
    <mu2 value="0.6"/>
    <kp value="10000000.0" />
    <kd value="1.0" />
    <fdir1 value="0 1 0"/>
  </gazebo>

  <xacro:mecanum_wheel name="front_right" side="1"  interface="hardware_interface/EffortJointInterface"/>
  <xacro:mecanum_wheel name="front_left"  side="-1" interface="hardware_interface/EffortJointInterface"/>
  <xacro:mecanum_wheel name="rear_right"  side="-1" interface="hardware_interface/EffortJointInterface"/>
  <xacro:mecanum_wheel name="rear_left"   side="1"  interface="hardware_interface/EffortJointInterface"/>

  <joint name="front_right_wheel_joint" type="continuous">    
    <origin xyz="0.3 -0.25 -0.1" rpy="0 0 ${pi/2}" />
    <axis xyz="1 0 0" />
    <parent link="base_link" />
    <child link="front_right_wheel_link" />    
  </joint>

  <joint name="front_left_wheel_joint" type="continuous">
    <origin xyz="0.3 0.25 -0.1" rpy="0 0 ${pi/2}" />
    <axis xyz="1 0 0" />
    <parent link="base_link" />
    <child link="front_left_wheel_link" />
  </joint>

  <joint name="rear_right_wheel_joint" type="continuous">
    <origin xyz="-0.3 -0.25 -0.1" rpy="0 0 ${pi/2}" />
    <axis xyz="1 0 0" />
    <parent link="base_link" />
    <child link="rear_right_wheel_link" />
  </joint>

  <joint name="rear_left_wheel_joint" type="continuous">
    <origin xyz="-0.3 0.25 -0.1" rpy="0 0 ${pi/2}" />
    <axis xyz="1 0 0" />
    <parent link="base_link" />
    <child link="rear_left_wheel_link" />
  </joint>


  <joint name="hokuyo_joint" type="fixed">
     <origin xyz="-0.0 0 0.19" rpy="0 0 3.1415926"/>
     <axis xyz="0 1 0" rpy="0 0 0"/>
     <parent link="base_link"/>
     <child link="hokuyo"/>
     <limit effort="10000" velocity="1000"/>
     <dynamics damping="1.0" friction="1.0"/>
  </joint>

  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/</robotNamespace>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
      <legacyModeNS>true</legacyModeNS>
    </plugin>
  </gazebo>
</robot>

controller.yaml

joint_state_controller:
  type: "joint_state_controller/JointStateController"
  publish_rate: 50 

front_right_controller:
  type: effort_controllers/JointVelocityController
  joint: front_right_wheel_joint  
  pid: {p: 10.0, i: 0.01, d: 0.0}

front_left_controller:
  type: effort_controllers/JointVelocityController
  joint: front_left_wheel_joint  
  pid: {p: 10.0, i: 0.01, d: 0.0}

rear_right_controller:
  type: effort_controllers/JointVelocityController
  joint: rear_right_wheel_joint  
  pid: {p: 10.0, i: 0.01, d: 0.0}

rear_left_controller:
  type: effort_controllers/JointVelocityController
  joint: rear_left_wheel_joint  
  pid: {p: 10.0, i: 0.01, d: 0.0}

These are my current main three pieces of code. I can currently control the movement of the four wheels, but I've attempted to create an Odometry subscriber. The issue is that it cannot read the coordinates, causing the car to not stop after reaching the destination. I've tried using rostopic info and rostopic echo to check. The results show that the odom subscriber is indeed set up, but it's not receiving any messages.