Select start pose option

[Marc-Morcos]

I have done some visualization of the planned paths. It seems the path starts at an arbitrary point near the left of the polygon, (not the corner of the polygon where the bot is). Is there a way to control this? If not, is there a way to get the start pose before path execution starts so that we can navigate to it?

[SteveMacenski]

An image of what you mean would be helpful and detail about which node you're using. The paths of the field now (not row node) should start at one of the corners of the field, minus the headlands and the tool's radius. So there are different variant options.

v1.X in F2C includes the ability to modify the variant to use of those options in the API [1], however, there's not really a way to know which is which. So you'd basically just have to guess and check or generate all possible variants and then sort it out yourself as to which you want to use. For that reason, I chose not to expose that feature since it wasn't knowable beforehand & you could get the same effect just by ordering the points differently in your input file (i.e. roll the points' positions in the inputs).

But, I did think to myself someone might eventually want that feature, so I didn't do anything that prevents a contributor from adding in which variant to use in the request field and propagating it to the route planner of choice. Or I suppose we could modify the request to add an option for generate_all_variants and return a vector of fields to choose from.

[1] https://github.com/Fields2Cover/Fields2Cover/blob/v1.2.0/include/fields2cover/route_planning/single_cell_swaths_order_base.h#L22

is there a way to get the start pose before path execution starts so that we can navigate to it?

Yeah! Look at the path and take its first point path.poses[0].pose.position and navigate there with Nav2 before then sending the rest of the plan.

[Marc-Morcos]

Thanks for the reply, I am essentially using the demo, https://github.com/open-navigation/opennav_coverage/tree/iron/opennav_coverage_demo.

Yeah! Look at the path and take its first point path.poses[0].pose.position and navigate there with Nav2 before then sending the rest of the plan.

I mean through the python interface. All I see is

goal_msg = NavigateCompleteCoverage.Goal()
goal_msg.frame_id = frame_id
goal_msg.polygons.append(self.toPolygon(field))
self.coverage_client.send_goal_async(goal_msg, self._feedbackCallback)

Am I supposed to use a compute message first? If so, then how do I run the computed path?

get the same effect just by ordering the points differently in your input file (i.e. roll the points' positions in the inputs).

I tried rotating the input array, but no matter what is starts at the point with the lowest x value.

[SteveMacenski]

You'd need to create the appropriate behavior tree to do that - that's the power of Nav2 to allow you to modify the navigation logic to enable something like Navigating for Coverage in the first place - but it should be straight forward for what you need. Have a sequence that is: Plan a route stored in plan -> send the first point in plan to navigation as a goal -> continue with the rest of the BT. This is the current BT [1], its really trivial for demonstration purposes.

You'll want to / need to extend it to your particular application desires - I'd love to add a new BT in that does what we discussed once you've put it together. I'm happy to merge that PR so that others can have that as well!

[1] https://github.com/open-navigation/opennav_coverage/blob/main/opennav_coverage_bt/behavior_trees/navigate_w_basic_complete_coverage.xml

[Marc-Morcos]

I see. I'm not very familiar with behavior trees. I tried setting up 2 navigators.
navigators: ['navigate_complete_coverage',"navigate_to_pose"]
but I'm not sure how to make a behavior tree that is compatible with both. Here is what I currently have, but I get an error saying I can't open it

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RateController hz="0.0000001"> <!-- once, for demo -->
      <Sequence name="NavigateWithoutReplanning">
        <ComputeCoveragePath nav_path="{path}" polygons="{field_polygon}" polygons_frame_id="{polygon_frame_id}" error_code_id="{compute_coverage_error_code}"/>
        <ComputePathToPose goal="{path.poses[0]}" path="{pathToStart}" planner_id="GridBased" error_code_id="{compute_path_error_code}"/>
        <FollowPath path="{pathToStart}" controller_id="FollowPath" error_code_id="{follow_path_error_code}"/>
        <FollowPath path="{path}" controller_id="FollowPath" error_code_id="{follow_path_error_code}"/>
      </Sequence>
    </RateController>
  </BehaviorTree>
</root>

[Marc-Morcos]

Wait, it does almost work, but I get it doesnt seem to be processing the goal properly
GridBased plugin failed to plan from (-0.01, -0.01) to (0.00, 0.00): "Unable to transform poses to global frame"

[SteveMacenski]

That generally look about right!

Is the goal at either of those coordinates? Might be worth printing some out. What's the global frame and is that in your TF tree? Its possible that the path.poses[0] doesn't have its header populated, so you'd have to add that from the path.header. It might be worth creating a BT node to take in the {path} and return a {goal} which is path.poses[0] with the header populated. I think that would actually be a general purpose node worth of adding directly into Nav2! Make it an option to select the first or last point and I could see that having numerous use cases.

I'm surprised that you could get {path.poses[0]} to work -- but its also possible that it actually doesn't work and its just populating an empty PoseStamped() which is why you see the coordinates (0.00, 0.00) and unable to transform since the frame/stamp would also be empty string / 0. So... perhaps it doesn't functionally work, even if it seems to at first glance have the right types.

[Marc-Morcos]

Ya if I input any nonsense in that field I get 0,0. So its not working, thats just the default value.

[SteveMacenski]

Got it - a new BT node for that should be easy. Here's an example: https://github.com/ros-navigation/navigation2/blob/main/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp the "work" parts of it are different, but shows how to write the BT node and get/set the input and output ports with a path

[Marc-Morcos]

So I got it all working, although if the start point is too far, I get the following error. Any idea on how to fix it? I assume its a config thing on my side. GridBased plugin failed to plan from (0.00, -0.01) to (-105.92, 39.25): "Goal Coordinates of(-105.918845, 39.251424) was outside bounds"

[SteveMacenski]

I assume outside the bounds of the costmap? You just need to set that size appropriately for the field you’re in

[Marc-Morcos]

Thanks. I guess once the pull requests are merged we can close the issue

[SteveMacenski]

These look great! Merged! Thanks for the contributions!