Unable to control thrusters using 'cmd_ve'l topic in MAVROS

General Discussion
1

Hello guys ,

We have bulit an ASV (Autonomus Surface Vehicle) with a two thruster (T200) configuration. The flight controller used is Pixhawk 6C with the ArduRover firmware and Jetson Orin as the companion computer. I am trying to control my motors in GUIDED mode by sending velocity commands to ‘/mavros/setpoint_velocity/cmd_vel’ but there is no response from the motors. I also tried the same in MANUAL mode to see of there is any difference but its still the same.

The follwoing is the code I am using:

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import TwistStamped
import math

rospy.init_node('shape_mover', anonymous=True)
rate = rospy.Rate(10)  # 10 Hz

cmd_vel_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10)
forward_cmd = TwistStamped() #message type

def forward(dist, speed):
    time = dist/speed
    time = int(time)
    for i in range(time):
        # Move forward
        #forward_cmd = TwistStamped()
        forward_cmd.twist.linear.x = speed
        forward_cmd.twist.linear.y = 0.0
        forward_cmd.twist.linear.z = 0.0  # Adjust the linear speed as needed
        cmd_vel_pub.publish(forward_cmd)
        rospy.sleep(1)  # Move forward for 1 second

def turn_left(angle, time): #Rotate about the z axis
    r_angle = math.radians(angle)
    turn_rate = r_angle/time
    for j in range(time):
        forward_cmd.twist.angular.x = 0.0
        forward_cmd.twist.angular.y = 0.0
        forward_cmd.twist.angular.z = turn_rate + 0.35 #Compensating the error by adding a constant
        cmd_vel_pub.publish(forward_cmd)
        rospy.sleep(1)

def stop(): #Stop the boat completely
    forward_cmd.twist.linear.x = 0.0
    forward_cmd.twist.linear.y = 0.0
    forward_cmd.twist.linear.z = 0.0
    forward_cmd.twist.angular.x = 0.0
    forward_cmd.twist.angular.y = 0.0
    forward_cmd.twist.angular.z = 0.0
    cmd_vel_pub.publish(forward_cmd)
    rospy.sleep(2)  

def move_in_shape():
    #Function for pattern navigation
    forward(30,2) #parameters: distance to move forward in meters, speed in meters/sec
    stop()
    turn_left(90,3) #parameters: turn angle in degrees, time required to turn in sec
    print("First turn")
    stop()
    forward(30,2)
    stop()
    turn_left(90, 3)
    print("Second turn")
    stop()
    forward(30,2)
    stop()
    turn_left(90, 3)
    print("Third turn")
    stop()
    forward(30, 2)
    stop()

if __name__ == '__main__':
    try:
        move_in_shape()
    except rospy.ROSInterruptException:
        pass

It would be a great help if someone can suggest where I am going wrong.

Thanks,
Naveen