Hi All,
I am working on a trajectory generation and testing with ArduCopter. my hardware setup is Cube Orange with Raspberry pi4 (8gb) as Onboard computer(with Ubuntu 20 Server). ROS and MAVROS are installed with R-Pi, I wanted to fly drone through a set of waypoints and ill keep updating this waypoints. Using the Mavros topic “/mavros/trajectory/generated” i can able to publish the waypoints to the ros but arducopter is not responding to this trajectory message. here is the code i testing
@ sprained_ankleEric Johnson can you help me on this
#!/usr/bin/env python3
import rospy
from mavros_msgs.msg import Trajectory, PositionTarget
from mavros_msgs.srv import CommandBool, SetMode, CommandTOL
def generate_trajectory():
rospy.init_node(‘publish_trajectory’)
trajectory_pub = rospy.Publisher('/mavros/trajectory/generated', Trajectory, queue_size=10)
current_time = rospy.Time.now()
trajectory_msg = Trajectory()
rate = rospy.Rate(1)
print("main")
while not rospy.is_shutdown():
# print("while")
# Create a new set of trajectory points when the previous set is reached by the drone
if rospy.Time.now() - current_time >= rospy.Duration.from_sec(5):#set trajectory generation intervel aswell as publilshing intervel
# Create a Trajectory message
trajectory_msg = Trajectory()
# Create 3 TrajectoryPoint messages and add them to the Trajectory message
trajectory_msg.point_1 = PositionTarget()
trajectory_msg.point_1.coordinate_frame = PositionTarget.FRAME_LOCAL_NED
trajectory_msg.point_1.type_mask = PositionTarget.IGNORE_YAW_RATE + PositionTarget.IGNORE_AFZ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + \
PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ
trajectory_msg.point_1.header.frame_id = 'map'
trajectory_msg.point_1.header.stamp = rospy.Time.now()
# trajectory_msg.point_1.
trajectory_msg.point_1.position.x = 10
trajectory_msg.point_1.position.y = 2.0
trajectory_msg.point_1.position.z = 15.0
trajectory_msg.point_1.velocity.x = 10
trajectory_msg.point_1.velocity.y = 2.0
trajectory_msg.point_1.velocity.z = 15.0
trajectory_msg.point_2 = PositionTarget()
trajectory_msg.point_2.coordinate_frame = PositionTarget.FRAME_LOCAL_NED
trajectory_msg.point_2.type_mask = PositionTarget.IGNORE_YAW_RATE + PositionTarget.IGNORE_AFZ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + \
PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ
# point1.time_from_start = rospy.Duration.from_sec(1.0)
trajectory_msg.point_2.header.frame_id = 'map'
trajectory_msg.point_2.header.stamp = rospy.Time.now()
trajectory_msg.point_2.position.x = 20
trajectory_msg.point_2.position.y = 2.0
trajectory_msg.point_2.position.z = 15.0
trajectory_msg.point_3 = PositionTarget()
trajectory_msg.point_3.coordinate_frame = PositionTarget.FRAME_LOCAL_NED
trajectory_msg.point_3.type_mask = PositionTarget.IGNORE_YAW_RATE + PositionTarget.IGNORE_AFZ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + \
PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ
# point1.time_from_start = rospy.Duration.from_sec(1.0)
trajectory_msg.point_3.header.frame_id = 'map'
trajectory_msg.point_3.header.stamp = rospy.Time.now()
trajectory_msg.point_3.position.x = 30
trajectory_msg.point_3.position.y = 2.0
trajectory_msg.point_3.position.z = 15.0
trajectory_msg.point_4 = PositionTarget()
trajectory_msg.point_4.coordinate_frame = PositionTarget.FRAME_LOCAL_NED
trajectory_msg.point_4.type_mask = PositionTarget.IGNORE_YAW_RATE + PositionTarget.IGNORE_AFZ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + \
PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ
# point1.time_from_start = rospy.Duration.from_sec(1.0)
trajectory_msg.point_4.header.frame_id = 'map'
trajectory_msg.point_4.header.stamp = rospy.Time.now()
trajectory_msg.point_4.position.x = 40
trajectory_msg.point_4.position.y = 2.0
trajectory_msg.point_4.position.z = 15.0
trajectory_msg.point_5 = PositionTarget()
trajectory_msg.point_5.coordinate_frame = PositionTarget.FRAME_LOCAL_NED
trajectory_msg.point_5.type_mask = PositionTarget.IGNORE_YAW_RATE + PositionTarget.IGNORE_AFZ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + \
PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ
# point1.time_from_start = rospy.Duration.from_sec(1.0)
trajectory_msg.point_5.header.frame_id = 'map'
trajectory_msg.point_5.header.stamp = rospy.Time.now()
trajectory_msg.point_5.position.x = 40
trajectory_msg.point_5.position.y = 2.0
trajectory_msg.point_5.position.z = 15.0
trajectory_msg.point_valid = [True, True, True, True, True]
trajectory_msg.type = 0
print("published")
trajectory_pub.publish(trajectory_msg)
rate.sleep()
if name == ‘main’:
try:
generate_trajectory()
except rospy.ROSInterruptException:
pass
am i missing something or this issue is related with the configuration?, please helpme