Publishing multiple waypoints at once?

[kadhirumasankar]

Hi. In 1:05 of your demo video (https://youtu.be/UKoaGW7t7Dk?t=65) I noticed that 4 waypoints are being published at the same time, and ego_planner is being commanded to reach each waypoint in order. What topic would I have to publish to to get this functionality?

So far, I have tried publishing a path to /waypoint_generator/waypoints, because when looking at the code in ego_replan_fsm.cpp I noticed that this topic takes a Path message. However, when I try this, only the first point appears in Rviz, and the drone only navigates to the first point.

This is a snippet of my code:

        self.path_publisher = rospy.Publisher(
            "/waypoint_generator/waypoints", Path, queue_size=1
        )

        self.path_msg = Path()
        self.path_msg.header.seq = 1
        self.path_msg.header.stamp = rospy.Time.now()
        self.path_msg.header.frame_id = "world"
        self.path_msg.poses.append(PoseStamped())
        self.path_msg.poses[0].header.seq = 2
        self.path_msg.poses[0].header.stamp = rospy.Time.now()
        self.path_msg.poses[0].header.frame_id = "map"
        self.path_msg.poses[0].pose.position.x = 5.0
        self.path_msg.poses[0].pose.position.y = 0.0
        self.path_msg.poses[0].pose.position.z = 1.5
        self.path_msg.poses.append(PoseStamped())
        self.path_msg.poses[1].header.seq = 3
        self.path_msg.poses[1].header.stamp = rospy.Time.now()
        self.path_msg.poses[1].header.frame_id = "map"
        self.path_msg.poses[1].pose.position.x = 5.0
        self.path_msg.poses[1].pose.position.y = 5.0
        self.path_msg.poses[1].pose.position.z = 1.5
        self.path_msg.poses.append(PoseStamped())
        self.path_msg.poses[2].header.seq = 4
        self.path_msg.poses[2].header.stamp = rospy.Time.now()
        self.path_msg.poses[2].header.frame_id = "map"
        self.path_msg.poses[2].pose.position.x = 0.0
        self.path_msg.poses[2].pose.position.y = 5.0
        self.path_msg.poses[2].pose.position.z = 1.5

        # In a different function
        self.path_publisher.publish(self.path_msg)

What am I doing wrong? Any kind of help would be greatly appreciated

[bigsuperZZZX]

Sorry for replying so late, since I have been working on a paper submission recently. From the function EGOReplanFSM::waypointCallback, only the first point in topic /waypoint_generator/waypoints is processed. If you want to let the drone fly through multiple waypoints, you can set the flight_type in run_in_sim.launch or simple_run.launch to 2 and manually set each waypoints afterwards in launch files. If you must give the waypoints during the flight using topics, I recommend you publish each waypoint in each message. You will need to determine the time to publish each waypoint.

[kadhirumasankar]

Thank you. I set flight_type to 2 and added points to the launch file. Thanks for your help.

[Kaapeine]

Hi, does the drone pass through all the waypoints in your case, with a large (10+) number of them?

[bigsuperZZZX]

You could add more points like our code: