Hi, I want to send a trajectory for the drone to follow.
Here is my code:
#! /usr/bin/env python3
import rospy
import math
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State, Trajectory
from mavros_msgs.srv import CommandBool, CommandBoolRequest, SetMode, SetModeRequest
current_state = State()
def state_cb(msg):
global current_state
current_state = msg
def fillUnusedTrajectorySetpoints(point):
point.position.x = math.nan
point.position.y = math.nan
point.position.z = math.nan
point.velocity.x = math.nan
point.velocity.y = math.nan
point.velocity.z = math.nan
point.acceleration_or_force.x = math.nan;
point.acceleration_or_force.y = math.nan;
point.acceleration_or_force.z = math.nan;
point.yaw = math.nan;
point.yaw_rate = math.nan;
return point
if __name__ == "__main__":
rospy.init_node("offb_node_py")
state_sub = rospy.Subscriber("mavros/state", State, callback = state_cb)
local_pos_pub = rospy.Publisher("mavros/setpoint_position/local", PoseStamped, queue_size=10)
traj_pub = rospy.Publisher("mavros/trajectory/generated", Trajectory, queue_size=10)
rospy.wait_for_service("/mavros/cmd/arming")
arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)
rospy.wait_for_service("/mavros/set_mode")
set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode)
# Setpoint publishing MUST be faster than 2Hz
rate = rospy.Rate(20)
# Wait for Flight Controller connection
while(not rospy.is_shutdown() and not current_state.connected):
rate.sleep()
pose = PoseStamped()
pose.pose.position.x = 5
pose.pose.position.y = 5
pose.pose.position.z = 10
setpoint = Trajectory()
setpoint.header.stamp = rospy.Time.now()
setpoint.header.frame_id = "local_origin"
setpoint.type = 0
setpoint.point_1.position.x = 5
setpoint.point_1.position.y = 5
setpoint.point_1.position.z = 10
setpoint.point_1.velocity.x = math.nan
setpoint.point_1.velocity.y = math.nan
setpoint.point_1.velocity.z = math.nan
setpoint.point_1.acceleration_or_force.x = math.nan
setpoint.point_1.acceleration_or_force.y = math.nan
setpoint.point_1.acceleration_or_force.z = math.nan
setpoint.point_1.yaw = math.nan
setpoint.point_1.yaw_rate = math.nan
setpoint.point_2.position.x = -5
setpoint.point_2.position.y = -5
setpoint.point_2.position.z = 5
setpoint.point_2.velocity.x = math.nan
setpoint.point_2.velocity.y = math.nan
setpoint.point_2.velocity.z = math.nan
setpoint.point_2.acceleration_or_force.x = math.nan
setpoint.point_2.acceleration_or_force.y = math.nan
setpoint.point_2.acceleration_or_force.z = math.nan
setpoint.point_2.yaw = math.nan
setpoint.point_2.yaw_rate = math.nan
# fillUnusedTrajectorySetpoints(setpoint.point_2)
fillUnusedTrajectorySetpoints(setpoint.point_3)
fillUnusedTrajectorySetpoints(setpoint.point_4)
fillUnusedTrajectorySetpoints(setpoint.point_5)
setpoint.time_horizon = [math.nan, math.nan, math.nan, math.nan, math.nan]
setpoint.point_valid = [1, 1, 0, 0, 0]
# Send a few setpoints before starting
for i in range(100):
if(rospy.is_shutdown()):
break
traj_pub.publish(setpoint)
local_pos_pub.publish(pose)
rate.sleep()
offb_set_mode = SetModeRequest()
offb_set_mode.custom_mode = 'OFFBOARD'
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
last_req = rospy.Time.now()
while(not rospy.is_shutdown()):
if(current_state.mode != "OFFBOARD" and (rospy.Time.now() - last_req) > rospy.Duration(5.0)):
if(set_mode_client.call(offb_set_mode).mode_sent == True):
rospy.loginfo("OFFBOARD enabled")
last_req = rospy.Time.now()
else:
if(not current_state.armed and (rospy.Time.now() - last_req) > rospy.Duration(5.0)):
if(arming_client.call(arm_cmd).success == True):
rospy.loginfo("Vehicle armed")
last_req = rospy.Time.now()
traj_pub.publish(setpoint)
# local_pos_pub.publish(pose)
rate.sleep()
But PX4 not allow me to arm. How can i send trajectory to drone?
Thank you.