Thank you for your response. After running the command rostopic echo /mavros/local_position/pose I was able to confirm that the position from the FMU is accurate. Additionally, when connected to the Realsense T265, the position is also correct. However, after reviewing the log file, I noticed that the magnetic field on the z-axis is strong. Could this be a potential cause for the issue at hand?
Here is the position control script. The script takeoff_landing.py publish the position to the topic /object_detection/local_position, the main script subscribe to that topic and then publish it to /mavros/setpoint_position/local
#!/usr/bin/env python
‘’’
@DiegoHerrera
‘’’
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Pose
from time import sleep
from nav_msgs.msg import Odometry
from gazebo_msgs.msg import ModelStates
class drone_1:
def takeoff_task(self, data):
self.takeoff = data.data
if self.takeoff == 'ready':
self.takeoff_flag = True
if self.takeoff == 'done':
self.takeoff_flag = False
return self.takeoff_flag
def orientation_zed_callback(self, msg):
self.pose_pos_x = msg.pose.pose.position.x
self.pose_pos_y = msg.pose.pose.position.y
self.pose_pos_z = msg.pose.pose.position.z
def orientation_t265_callback(self, msg):
self.pose_pos_x = msg.pose.pose.position.x
self.pose_pos_y = msg.pose.pose.position.y
self.pose_pos_z = msg.pose.pose.position.z
def orientation_callback(self, msg):
self.pose_pos_x = msg.pose[1].position.x
self.pose_pos_y = msg.pose[1].position.y
self.pose_pos_z = msg.pose[1].position.z
def __init__(self):
rospy.Subscriber("/object_detection/model_ready", String, self.takeoff_task)
rospy.Subscriber("/camera/odom/sample", Odometry, self.orientation_t265_callback)
#rospy.Subscriber("/zedm/zed_node/odom",Odometry, self.orientation_zed_callback)
#rospy.Subscriber("/gazebo/model_states",ModelStates, self.orientation_callback)
pub = rospy.Publisher('/object_detection/local_position', Pose, queue_size=10)
landing_pub = rospy.Publisher('/object_detection/landing', String, queue_size=10)
self.takeoff = ''
self.takeoff_flag = False
self.landing_flag = False
self.initial_position = 0.14
self.zVal = 0.70
self.pose_pos_x = 0
self.pose_pos_y = 0
self.pose_pos_z = 0
self.position_value_z = 0
self.rate = rospy.Rate(10) # 10hz
self.d = rospy.Duration(0.5)
self.pose = Pose()
rate = rospy.Rate(20)
self.count = 0
self.count2 = 0
while not rospy.is_shutdown():
if self.takeoff_flag:
# rospy.loginfo("Count equal to: %f" % self.count)
rospy.loginfo("Takingoff...")
self.pose.position.x = 0.0
self.pose.position.y = 0.0
self.pose.position.z = 0.70
self.pose.orientation.x = 0 # -0.018059104681
self.pose.orientation.y = 0 # 0.734654724598
self.pose.orientation.z = 0 # 0.00352329877205
self.pose.orientation.w = 1 # 0.678191721439
pub.publish(self.pose)
rospy.loginfo("altitude equal to: %f" % self.pose_pos_z)
# self.zVal += 0.01
if self.pose_pos_z >= 0.68:
self.takeoff_flag = False
self.landing_flag = True
sleep(15)
if self.takeoff_flag==False and self.landing_flag==True:
rospy.loginfo("Landing...")
self.pose.position.x = 0.0
self.pose.position.y = -0.0
self.pose.position.z = 0.30
self.pose.orientation.x = 0 # -0.018059104681
self.pose.orientation.y = 0 # 0.734654724598
self.pose.orientation.z = 0 # 0.00352329877205
self.pose.orientation.w = 1 # 0.678191721439
#print(self.zVal)
pub.publish(self.pose)
sleep(.3)
rospy.loginfo("altitude equal to: %f" % self.pose.position.z)
self.pose.position.x = 0.0
self.pose.position.y = -0.0
self.pose.position.z = 0.12
self.pose.orientation.x = 0 # -0.018059104681
self.pose.orientation.y = 0 # 0.734654724598
self.pose.orientation.z = 0 # 0.00352329877205
self.pose.orientation.w = 1 # 0.678191721439
pub.publish(self.pose)
if self.pose_pos_z <= 0.18:
rospy.loginfo("altitude GAZEBO to: %f" % self.pose_pos_z)
self.takeoff_flag = False
self.landing_flag = False
msg = "Landing"
landing_pub.publish(msg)
break
rate.sleep()
if name == “main”:
rospy.init_node(‘takingoff_node’, anonymous=True)
rospy.loginfo(“Takeoff_landing node ready”)
try:
drone_class = drone_1()
except rospy.ROSInterruptException:
raise
Here is the main node that publish to /mavros/setpoint_position/local
import rospy
import mavros
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
from std_msgs.msg import String
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Pose
from time import sleep
current_state = State() # Reading the current state from mavros msgs
mavros.set_namespace()
class drone_control:
def state_callback(self, state):
self.current_state = state
def coordinates_orientation_2(self, data_2):
# print('data of Face Found: ', data_2)
self.pose.header.stamp = rospy.Time.now()
self.pose.pose.position.x = data_2.position.x
self.pose.pose.position.y = data_2.position.y
self.pose.pose.position.z = data_2.position.z
# quat = quaternion_from_euler(rVal2, pVal2, yawVal2) # + pi_2)
self.pose.pose.orientation.x = data_2.orientation.x # quat[0]
self.pose.pose.orientation.y = data_2.orientation.y # quat[1]
self.pose.pose.orientation.z = data_2.orientation.z # quat[2]
self.pose.pose.orientation.w = data_2.orientation.w # quat[3]
def land_callback(self, msg):
self.flag = True
def __init__(self):
self.flag = False
self.pose = PoseStamped()
self.local_position_publisher = rospy.Publisher(mavros.get_topic('setpoint_position', 'local'), PoseStamped,
queue_size=10) #
self.sub = rospy.Subscriber("/object_detection/local_position", Pose, self.coordinates_orientation_2)
self.state_subscriber = rospy.Subscriber(mavros.get_topic('state'), State, self.state_callback)
self.arming_client = rospy.ServiceProxy(mavros.get_topic('cmd', 'arming'), CommandBool)
self.takeoff_client = rospy.ServiceProxy(mavros.get_topic('cmd', 'takeoff'), CommandTOL)
# landing_client = rospy.ServiceProxy(mavros.get_topic('cmd', 'land'), CommandTOL)
self.set_mode_client = rospy.ServiceProxy(mavros.get_topic('set_mode'), SetMode)
#self.current_state = ''
print("Position control function")
self.prev_state = self.current_state
rate = rospy.Rate(20.0)
for i in range(100):
self.local_position_publisher.publish(self.pose)
rate.sleep()
while not self.current_state.connected:
rate.sleep()
self.last_request = rospy.get_rostime()
while not rospy.is_shutdown():
now = rospy.get_rostime()
# qxVal, yVal, zVal, rVal, pVal, yawVal = 0 , 0, 1.6, 0, 0, 0
if current_state.mode != "OFFBOARD" and (now - self.last_request > rospy.Duration(5.)):
self.set_mode_client(base_mode=0, custom_mode="OFFBOARD")
last_request = now
else:
if not self.current_state.armed and (now - self.last_request > rospy.Duration(5.)):
self.arming_client(True)
last_request = now
if self.current_state.armed:
print("Drone ready to fly")
if self.prev_state.armed != current_state.armed:
rospy.loginfo("Vehicle armed: %r" % current_state.armed)
if self.prev_state.mode != current_state.mode:
rospy.loginfo("Current mode: %s" % current_state.mode)
self.prev_state = current_state
self.pose.header.stamp = rospy.Time.now()
self.local_position_publisher.publish(self.pose)
rospy.Subscriber("/object_detection/landing", String, self.land_callback)
if self.flag:
self.set_mode_client(base_mode=0, custom_mode="AUTO.LAND")
sleep(8)
self.flag = False
break
rate.sleep()
if name == ‘main’:
try:
rospy.init_node(‘Quadcopter_node’, anonymous=True)
drone_control()
except rospy.ROSInterruptException:
pass
Do you have any suggestion to detect what is wrong? perhaps my code is not good?
