How to push multiple WP to PX4 SITL via mavros services?

JamesChooWK commented 4 years ago

This is only bug and feature tracker, please use it to report bugs or request features.

Issue details

I am trying to learn how to push several waypoint through mavros services ( ~mission/push (mavros_msgs/WaypointPush). I can manage to push just only one waypoint, but how can I push multiple waypoint? I wrote a script( shown below) but It does not do the trick and the second waypoint is not being pushed.

Thanks.

MAVROS version and platform

Mavros: 0.33.0 ROS: Kinetic Ubuntu: 16.04 Python: 2.7

Autopilot type and version

PX4

Version: 1.9.2

The function I wrote

from mavros_msgs.msg import Waypoint    # define waypoints
from mavros_msgs.srv import WaypointPush # push Waypoint

def waypt(self):
        if self.counter<3:
            # First waypoint
            wp = Waypoint()
            wp.frame = 3 # Global frame wth rel alt
            wp.command = 16 # Navigate to WayPoints
            wp.is_current = True
            wp.autocontinue = True
            wp.param1 = 0 # Hover in sec
            wp.param2 = 0 # Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached)
            wp.param3 = 0 # radius to consider passing through waypoint
            wp.param4 = float('nan') # Yaw angle at waypoint
            wp.x_lat = 2.909484
            wp.y_long = 101.655246
            wp.z_alt = 5
            self.setWayPoint(0, [wp])

            # Second Waypoint
            wp.frame = 3 # Global frame wth rel alt
            wp.command = 16 # Navigate to WayPoints
            wp.is_current = False
            wp.autocontinue = True
            wp.param1 = 0 # Hover in sec
            wp.param2 = 0 # Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached)
            wp.param3 = 0 # radius to consider passing through waypoint
            wp.param4 = float('nan') # Yaw angle at waypoint
            wp.x_lat = 2.909672
            wp.y_long = 101.655411
            wp.z_alt = 15
           self.setWayPoint(0, [wp])
           self.counter =4

def setWayPoint(self, index, waypoints):
        print('Set WayPoints \n')
        rospy.wait_for_service("mavros/mission/push")
        try:
            pushWayPointRequest = rospy.ServiceProxy("mavros/mission/push", WaypointPush)
            pushWayPointResponse = pushWayPointRequest(start_index=index, waypoints=waypoints)
            pushWayPoint_flag = pushWayPointResponse.success
            if pushWayPoint_flag == True:
                print('SUCCESS: Pushing waypoints \n')
            elif pushWayPoint_flag == False:
                print('FAILURE: Pushing waypoints \n')
        except rospy.ServiceException as e:
            rospy.loginfo("setWayPoint failed: %s\n" %e)

JamesChooWK commented 4 years ago