Hello.
QGroundcontrol says “communication lost” when I run code with loop in it.
I can give two examples:
1st example: I have 1st code that updates desired values through MavROS and when the first value is published the 1st code runs Mavlink command to arm the vehicle and this script terminates itself after that, while the 1st code keeps updating values (these values are not sent to the BlueROV2)
#!/usr/bin/env python3
import rospy
from std_msgs.msg import Float64
import math
import os
import subprocess
class AdHbMsgsNode:
def __init__(self):
rospy.init_node("ad_hb_msgs")
self.psi_ref_pub = rospy.Publisher("psi_ref_msg", Float64, queue_size=10)
self.z_ref_pub = rospy.Publisher("z_ref_msg", Float64, queue_size=10)
self.x_lat_des_pub = rospy.Publisher("x_des_msg", Float64, queue_size=10)
self.y_long_des_pub = rospy.Publisher("y_des_msg", Float64, queue_size=10)
self.n_rpm_pub = rospy.Publisher("n_rpm_msg", Float64, queue_size=10)
self.timer = rospy.Timer(rospy.Duration(1.0), self.calculate_and_publish_values)
# Geographic location of the trajectory (latitude, longitude)
self.trajectory_latitude = 51.5074 # Example latitude (London, UK)
self.trajectory_longitude = -0.1278 # Example longitude (London, UK)
# Previous x_lat_des and y_long_des values for calculating heading angle
self.prev_x_lat_des = 0.0
self.prev_y_long_des = 0.0
# Flag to indicate if the other script has been launched
self.arm_disarm_script_launched = False
def calculate_and_publish_values(self, event):
if not self.arm_disarm_script_launched:
# Get the full path to arm_disarm.py using os.path.expanduser() and os.path.join()
arm_disarm_path = os.path.expanduser("~/dev/mavros_bluerov2/ros_mavlink_bluerov_1ws/src/arm_disarm/src/arm_disarm.py")
# Launch the arm_disarm.py script as a separate process
subprocess.Popen(["python3", arm_disarm_path])
self.arm_disarm_script_launched = True
# Assign values to n_rpm
n_rpm = 1500
# Calculate the current time in seconds
current_time = rospy.Time.now().to_sec()
# Calculate the desired depth using a sine function
period = 40
amplitude_z = 1.0
frequency_z = 1.0 / period
z_ref = 250 + amplitude_z * math.sin(2 * math.pi * frequency_z * current_time)
# Calculate the desired x and y coordinates using different sine functions
amplitude_xy = 1.0 # Adjust this value as needed
frequency_xy = 1.0 / period # Adjust this value as needed
# Replace x_des with x_lat_des and convert meters to degrees latitude
x_lat_des_meters = 5.0 + amplitude_xy * math.sin(2 * math.pi * frequency_xy * current_time)
x_lat_des = self.trajectory_latitude + (x_lat_des_meters / 111111.0)
# Replace y_des with y_long_des and convert meters to degrees longitude
y_long_des_meters = -3.0 + amplitude_xy * math.sin(2 * math.pi * frequency_xy * current_time)
y_long_des = self.trajectory_longitude + (y_long_des_meters / (111111.0 * math.cos(self.trajectory_latitude * math.pi / 180.0)))
# Calculate the change in x_lat_des and y_long_des
delta_x_lat_des = x_lat_des - self.prev_x_lat_des
delta_y_long_des = y_long_des - self.prev_y_long_des
# Calculate the desired heading angle (psi_ref) based on the changes in x_lat_des and y_long_des
psi_ref = math.atan2(delta_y_long_des, delta_x_lat_des)
# Publish psi_ref, z_ref, x_lat_des, y_long_des, and n_rpm values to their respective topics
self.psi_ref_pub.publish(psi_ref)
self.z_ref_pub.publish(z_ref)
self.x_lat_des_pub.publish(x_lat_des)
self.y_long_des_pub.publish(y_long_des)
self.n_rpm_pub.publish(n_rpm)
# Update the previous values for the next iteration
self.prev_x_lat_des = x_lat_des
self.prev_y_long_des = y_long_des
rospy.loginfo("We are here:")
if __name__ == "__main__":
node = AdHbMsgsNode()
rospy.spin()
the 2nd code (executed by the first code):
# Import mavutil
from pymavlink import mavutil
import rospy
from std_msgs.msg import Float64
# Callback function for handling the received psi_ref messages
def psi_ref_callback(msg):
# Handle the received psi_ref message here
print("Received psi_ref:", msg.data)
# Create the connection
master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
# Wait a heartbeat before sending commands
master.wait_heartbeat()
# Subscribe to the psi_ref_msg topic
rospy.init_node("mavlink_subscriber")
rospy.Subscriber("psi_ref_msg", Float64, psi_ref_callback)
# Wait for some time to allow the subscriber to initialize
rospy.sleep(1)
# https://mavlink.io/en/messages/common.html#MAV_CMD_COMPONENT_ARM_DISARM
# Arm
# master.arducopter_arm() or:
master.mav.command_long_send(
master.target_system,
master.target_component,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
0,
1, 0, 0, 0, 0, 0, 0)
# wait until arming confirmed (can manually check with master.motors_armed())
print("Waiting for the vehicle to arm")
master.motors_armed_wait()
print('Armed!')
# Now start the rospy loop (This will keep the program running until it's terminated)
rospy.spin()
2nd example.
The script for controlling camera (found on your website) when I run it after 2 seconds the QGroundcontrol says “communication lost”. However, the script is being executed despite the lost connection.
import time
# Import mavutil
from pymavlink import mavutil
# Create the connection
master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
# Wait a heartbeat before sending commands
master.wait_heartbeat()
def look_at(tilt, roll=0, pan=0):
master.mav.command_long_send(
master.target_system,
master.target_component,
mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,
1,
tilt,
roll,
pan,
0, 0, 0,
mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING)
# cycles the camera up and down
while True:
for angle in range(-50, 50):
look_at(angle*100)
time.sleep(0.1)
for angle in range(-50, 50):
look_at(-angle*100)
time.sleep(0.1)
When I run a simple script that sends values to the BLueROV2 and then this script terminates itself the communication isn’t lost.
May you tell me why it is happening and what I need to do to avoid this problem? Can it be because when we send initial command with certain values and then we stop sending the values and instead of the values we send Null values that force the BlueROV2 to lose connection?
Thank you for your time.