Servers by jDrones

Cannot get ArduCopter 3.5.7 to takeoff with mavros topic /mavros/setpoint_attitude/thrust

Python Script for Publishing the Setpoint Attitude Thrust

import rospy
from mavros_msgs.srv import SetMode
from mavros_msgs.srv import CommandBool
from mavros_msgs.srv import CommandTOL
from mavros_msgs.msg import Thrust
import time

pub = rospy.Publisher('/mavros/setpoint_attitude/thrust', Thrust, queue_size=10)
rospy.init_node('mavros_takeoff_python')
rate = rospy.Rate(10)

# Set Mode
print "\nSetting Mode"
rospy.wait_for_service('/mavros/set_mode')
try:
    change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
    response = change_mode(custom_mode="GUIDED_NOGPS")
    rospy.loginfo(response)
except rospy.ServiceException as e:
    print("Set mode failed: %s" %e)

# Arm
print "\nArming"
rospy.wait_for_service('/mavros/cmd/arming')
try:
    arming_cl = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
    response = arming_cl(value = True)
    rospy.loginfo(response)
except rospy.ServiceException as e:
    print("Arming failed: %s" %e)

# Takeoff

thrust = Thrust()
thrust.thrust = 0.6
while not rospy.is_shutdown():
        rospy.loginfo(thrust)
        pub.publish(thrust)
        rate.sleep()

Output on Rostopic Echo on /mavros/setpoint_attitude/thrust when Python script is launched

pi@AirStone:~/Workspace/dronekit/Python $ python hover-land.py 

Setting Mode
[INFO] [1549234644.448172]: mode_sent: True

Arming
[INFO] [1549234644.482689]: success: True
result: 0
[INFO] [1549234644.488615]: header: 
  seq: 0
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: ''
thrust: 0.6
[INFO] [1549234644.519898]: header: 
  seq: 0
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: ''
thrust: 0.6
[INFO] [1549234644.619861]: header: 
  seq: 0
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: ''
thrust: 0.6
[INFO] [1549234644.720081]: header: 
  seq: 1
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: ''
thrust: 0.6

Please help fixing this issue. We are currently on a time crunch and would appreciate all the help possible.

Hello,

In GUIDED_NOGPS, I use the topic /mavros/setpoint_raw/atittude, where I provide the desired angle in quaternion (to be stabilize, you send x = 0, y = 0, z = 0, w = 1).
And you send the thrust, between 0 and 1 (so 0.5 = stay at the same altitude).

Here an example for publication :

       while not rospy.is_shutdown():
        qw,qx,qy,qz = euler_to_quaternion_angle(self.roll, self.pitch, self.yaw)
       
        AT = AttitudeTarget()
        AT.header = self.h
        AT.type_mask = 7
        AT.orientation.x = qx
        AT.orientation.y = qy
        AT.orientation.z = qz
        AT.orientation.w = qw
        AT.body_rate.x = 0
        AT.body_rate.y = 0
        AT.body_rate.z = 0
        AT.thrust = self.thrust
        
        self.pub.publish(AT)
        self.r.sleep() #For frequency

And the topic pub :
self.pub = rospy.Publisher("/mavros/setpoint_raw/attitude", AttitudeTarget, queue_size=10)#2

Hope it helps

Servers by jDrones