Hello guys ,
We have bulit an ASV (Autonomus Surface Vehicle) with a two thruster 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.