Nav2z client's waypoint Navigator sends goals in odom frame.

[NithishkumarS]

Bug Currently, I'm utilizing the nav2z smacc client for waypoint navigation on a differential drive. Specifically, I'm making use of CbNavigateNextWaypoint, which leverages the waypointNavigator component to transmit the goal as an action message to the Nav2 server. Upon investigation, I've discovered that the goals are sent in the odom frame, leading to unexpected goals in the map frame. This is not an issue when both the map and odom frame are same as in the start of a simulation. I would appreciate any recommendations on addressing this issue and modifying the frame of reference.

Expected behavior Waypoint navigation to the goal in Map frame.

Environment (please complete the following information):

ROS DETAILS:

• OS: Ubuntu 22.04 LTS
• Version: Humble

REPO DETAILS:

• Repository: robosoft-ai/SMACC2
• Branch: master

BUILD DETAILS

• Build Command: colcon build --symlink-install

Additional context

Upon closer examination of the code, I discovered that the sendNextGoal() function within the waypointNavigator component (*.cpp) utilizes the Pose object without any arguments, resulting in it defaulting to the odom frame of reference.

ClNav2Z::Goal goal;
auto p = client_->getComponent<cl_nav2z::Pose>();
auto pose = p->toPoseMsg();

// configuring goal
goal.pose.header.frame_id = p->getReferenceFrame();
//goal.pose.header.stamp = getNode()->now();
goal.pose.pose = next;

┆Issue is synchronized with this Jira Task by Unito

[pabloinigoblasco]

Hello @NithishkumarS

The implementation of navigation client behaviors use the Pose Component (CpPose). That component is created in one orthogonal at the begining of the execution. I guess you are doing something like this: https://github.com/robosoft-ai/SMACC2/blob/cc5904016f7da6d1f174f387341bf76b5756c068/smacc2_sm_reference_library/sm_dance_bot_warehouse_4/include/sm_dance_bot_warehouse_4/orthogonals/or_navigation.hpp#L56

You can use another reference frame using another constructor:

[class Pose : public smacc2::ISmaccComponent, public smacc2::ISmaccUpdatable](https://github.com/robosoft-ai/SMACC2/blob/cc5904016f7da6d1f174f387341bf76b5756c068/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/pose/cp_pose.hpp#L43)

Just add a the base link and the map_frame names. Something such as:

 nav2zClient->createComponent<Pose>("base_link", "map"); 

That should be enough to make it work, nonetheless, please, make me know if you find more problems.

[NithishkumarS]

nav2zClient->createComponent<Pose>("base_link", "map"); Making this changes to the Navigation orthogonal fixed the issue. Thanks for the quick response.