Multiple 2D nav goal

[angtzewern]

Is there any way to set multiple 2D nav goal and let it load in sequence?
from the husarion tutorial i can only set one 2d nav goal.

[lukasz.mitka]

Hi angtzewern,

You could use follow_waypoints node to load a sequence of goals.
If that is not enough, you could build your own node with use of actionlib library. Here you can find tutorial explaining how to use actionlib to send navigation goals.

Regards
Łukasz

[angtzewern]

Thank you for your guidance.
The above method seems to be working !

[urahara94]

is there a way we can fix the sequence of goal and save it and load it every time?
like pressing a button to reload those sequence

[lukasz.mitka]

Hi urahara94,

You could record a sequence of goals with rosbag and then play them every time you want to reload the sequence.

Regards,
Łukasz

[urahara94]

does this still maintain the obstacle avoidance and lidar function on the husarion?
or will this just repeat what has been done in the previous sequence?
so far i tested the follow_waypoints and upon completion the waypoint disappears

[lukasz.mitka]

This will maintain obstacle avoidance and other functions. Just remember to record and play only the waypoints topic.

[urahara94]

i tried out recording and playing the waypoint topic and i dont seem to be able to publish the waypoint to the waypoint node
it can replay it tho
attach is the waypoint bag (i attach it as c but change it back to waypoint.bag since this page dont allow me to post .bag format

waypoint.c (8.2 KB)

**update i found the solution where the follow_waypoint is publish at /initialpose instead of /waypoint (this is just indication)

[lukasz.mitka]

Yes, you’re right.

The /waypoints is for visualization.
What is required is an /initialpose topic with desired points and /path_ready topic to trigger the process.
It can be recorded with:

rosbag record initialpose path_ready -O points

Then played back with:

rosbag play points.bag 

Regards,
Łukasz

[urahara94]

ok thank you
but i notice another problem if i want to use amcl instead of gmapping.
It seems to clash with the initial pose setting. Is there anyway to disable the initial pose of amcl before setting the waypoint?

[lukasz.mitka]

Hello urahara94,

You could remap topic name for one of this nodes.
E.g. this could be:

rosrun follow_waypoints follow_waypoints initialpose:=points_to_follow

or use <remap> tag in launch file:

<remap from="initialpose" to="points_to_follow"/>

You will need to change topic name also for rosbag and rviz.

Regards,
Łukasz

[urahara94]

Hi lukasz

Ah i might have overlook the remap function. And yes this solved my issue i been having over the past week.

Thank you very much!

[angtzewern]

anyway i can remove skip the region which is empty and each it found a message it will execute and pause for 1 second before going to the next message.
having some issue with skipping the empty region and follow waypoint dont seem to be able to catch up with all the message at the same time

[lukasz.mitka]

Hello angtzewern,

Could you describe in more detail the problem you are facing?

Regards,
Łukasz

[angtzewern]

i want to play back a bagfile of the waypoint but it is 1 minute long
i do not want to wait for 1 minute
anyway i can skip all the empty frames or speed up the process?
currently i only increase the playback rate

[lukasz.mitka]

There are few options:

• Play bag file with option --immediate to publish all messages in file instantly
• Use rosbag filtering option to prepare new file with messages you want
• Use a Python API to make a script that will edit bagfile to suit your needs, this way you can edit any part of message or create a new bag from scratch.

Regards,
Łukasz

[angtzewern]

The cookbook seems interesting. I will try it out.
Thank you.
In addition to that i would like to ask any way we can make the follow waypoint auto stop until we send a new msg to move?
As in 4 waypoint set.
Each time it reach a waypoint it stops until a command is publish to call it move to second waypoint etc…

Thank you for all your help.
Regards,
Ang

[lukasz.mitka]

The follow_waypoint is built as a state machine, you could easily extend its functionality to your needs. In FollowPath class add a subscriber that will wait for the message to be published.

Regards,
Łukasz

[angtzewern]

i understand what you mean but i dont quite follow on how to change the functionality of the library.
any simpe example available?

[lukasz.mitka]

I prepared an example for you.
In follow_waypoints.py find class FollowPath(State) and modify its method execute(self, userdata):

    def execute(self, userdata):
        global waypoints
        # Execute waypoints each in sequence
        for waypoint in waypoints:
            # Break if preempted
            if waypoints == []:
                rospy.loginfo('The waypoint queue has been reset.')
                break
            trigger_topic = "/follow_trigger"
            self.wait_for_trigger = True            
            while self.wait_for_trigger:
                try:
                    trigger_msg = rospy.wait_for_message(trigger_topic, Bool, timeout=1)
                except rospy.ROSException as e:
                    if 'timeout exceeded' in e.message:
                        continue
                    else:
                        raise e
                rospy.loginfo("Proceed to next waypoint")
                self.wait_for_trigger = False
            goal = MoveBaseGoal()
            goal.target_pose.header.frame_id = self.frame_id
            goal.target_pose.pose.position = waypoint.pose.pose.position
            goal.target_pose.pose.orientation = waypoint.pose.pose.orientation
            rospy.loginfo('Executing move_base goal to position (x,y): %s, %s' %
                    (waypoint.pose.pose.position.x, waypoint.pose.pose.position.y))
            rospy.loginfo("To cancel the goal: 'rostopic pub -1 /move_base/cancel actionlib_msgs/GoalID -- {}'")
            self.client.send_goal(goal)
            self.client.wait_for_result()
        return 'success'

This way robot will wait for any message on /follow_trigger topic when navigating to each point.

To publish a message:

rostopic pub /follow_trigger std_msgs/Bool true

Regards,
Łukasz