armarx_navigation HowTos

Table of Contents

• How To
  • Usage of the Client API
    • Using the ComponentPluginUser
      • Component Dependencies
      • Configuring the Client in the Navigator
      • Sending Navigation Commands
    • Flexible Configuration
      • Component Dependencies
      • Configuring the Client in the Navigator
      • Sending Navigation Commands
• Usage Example
  • Skills
  • Example Client
• Scenarios

How To

Usage of the Client API

The following how-to guides explain how to send navigation commands to the navigator.

1. Using the ComponentPluginUser: This is the simplest approach and is ideal when a single configuration of the navigation stack is sufficient. It provides a straightforward way to interact with the navigator without requiring extensive customization.
2. Flexible Configuration: This approach offers greater flexibility by allowing multiple configurations of the navigation stack. It is suitable for scenarios where different navigation behaviors or advanced customizations are required.

Using the ComponentPluginUser

This example demonstrates how to implement a navigation client. It covers three key steps: (1) adding the navigator as a dependency to your component, (2) configuring your client, and (3) sending navigation commands.

Component Dependencies

To begin, your component needs access to the navigator component. Add the following header to the Component.h file to include the necessary symbols:

#include <armarx/navigation/client.h>

In the component's header file, include the following:

class Component: 
    virtual public armarx::armem::ListeningClientPluginUser,
    virtual public armarx::navigation::client::ComponentPluginUser
{
    // ...
}

The armarx::navigation::client::ComponentPluginUser provides access to the navigator.

The armem::ListeningClientPluginUser is essential for receiving updates from the memory, such as events. To enable this, extend the component's interface by modifying the ComponentInterface.ice file to include the MemoryListenerInterface:

#include <RobotAPI/interface/armem/client/MemoryListenerInterface.ice>
 
module your_component_namespace
{
    interface ComponentInterface extends armarx::armem::client::MemoryListenerInterface
    {
    // Define your interface here.
    };
};

Configuring the Client in the Navigator

The client must register itself with the navigator to specify the configuration of the navigation stack it will use. This configuration determines the behavior of navigation requests sent by the client.

Now, configure the navigation stack for your client. Start with the general configuration:

armarx::navigation::client::GeneralConfig generalParams{};

For global planning, use the following configuration:

armarx::navigation::global_planning::SPFAParams globalPlannerParams{};

In this example, local planning is not used. For the trajectory controller, configure parameters such as velocity limits:

armarx::navigation::traj_ctrl::local::TrajectoryFollowingControllerParams
        trajControllerParams{};
 
// Example: Restrict the platform's linear velocity
trajControllerParams.limits.linear = 420; // [mm/s]

Combine these configurations into a complete navigation stack configuration:

armarx::navigation::client::NavigationStackConfig cfg;
cfg.general(generalParams);
cfg.globalPlanner(globalPlannerParams);
cfg.trajectoryController(trajControllerParams);

Finally, register this configuration with the navigator server:

configureNavigator(cfg);

This step ensures that navigation requests from this client will use the specified configuration. You can verify this by checking the navigation memory (using the MemoryViewer widget), where an entry should now exist in the Navigation/Parameterization core segment with the name of your component.

Sending Navigation Commands

To move the robot, specify the target coordinates. For example, to navigate to the global position [1000, 1000]:

const Eigen::Isometry3f globalTarget(Eigen::Translation3f{1000, 1000, 0});

Send the navigation command:

ARMARX_INFO << "Sending navigation command";
getNavigator().moveTo({globalTarget}, core::NavigationFrame::Absolute);

The robot will start moving toward the specified target. To wait until the robot reaches its goal, use the following:

ARMARX_INFO << "Waiting until the robot reaches its goal ...";
 
const armarx::navigation::client::StopEvent stopEvent = getNavigator().waitForStop();
 
ARMARX_INFO << "The robot stopped.";

The StopEvent provides information about whether the goal was reached or if a failure occurred. For example:

if(stopEvent.isGoalReachedEvent())
{
    ARMARX_INFO << "The robot reached its goal";
}
else
{
    ARMARX_WARNING << "Something went wrong.";
}

Congratulations! You have successfully implemented a navigation client that can send commands and handle events. The robot should now navigate to the specified target or provide feedback if an issue arises.

Flexible Configuration

This example demonstrates how to implement a navigation client. It covers three key steps: (1) adding the navigator as a dependency to your component, (2) configuring your client, and (3) sending navigation commands.

Component Dependencies

To begin, your component needs access to the navigator component. Add the following header to the Component.h file to include the necessary symbols:

#include <armarx/navigation/client.h>

Next, add the navigator (armarx::navigation::client::NavigatorInterfacePrx) as a dependency to your component, along with the wrapper armarx::navigation::client::IceNavigator. The IceNavigator abstracts the underlying middleware (ZeroC Ice) and implements the armarx::navigation::core::NavigatorInterface.

In the component's header file, include the following:

class Component: 
    virtual public armarx::armem::ListeningClientPluginUser
{
    // ...
 
    private:
    armarx::navigation::client::NavigatorInterfacePrx navigatorPrx;
    armarx::navigation::client::IceNavigator iceNavigator;
}

The armem::ListeningClientPluginUser is essential for receiving updates from the memory, such as events. To enable this, extend the component's interface by modifying the ComponentInterface.ice file to include the MemoryListenerInterface:

#include <RobotAPI/interface/armem/client/MemoryListenerInterface.ice>
 
module your_component_namespace
{
    interface ComponentInterface extends armarx::armem::client::MemoryListenerInterface
    {
    // Define your interface here.
    };
};

In the source file, register the navigator as a dependency:

armarx::PropertyDefinitionsPtr
Component::createPropertyDefinitions()
{
    armarx::PropertyDefinitionsPtr def =
    new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
 
    // Add the navigator as a dependency
    def->component(navigatorPrx, "navigator");
}

Then, connect the navigator to the IceNavigator:

void
Component::onConnectComponent()
{
    iceNavigator.setNavigatorComponent(navigatorPrx);
}

At this point, your component can connect to the navigator. However, you still need to configure your client to define the desired behavior, such as the robot's maximum velocity.

Configuring the Client in the Navigator

The client must register itself with the navigator to specify the configuration of the navigation stack it will use. This configuration determines the behavior of navigation requests sent by the client.

First, set the name of the client. This ensures that only events relevant to this client are handled:

const std::string configId = getName(); // Use the component's name

Next, instantiate the event subscriber to handle planned events (e.g., movement started, goal reached) and failures:

armarx::navigation::client::MemorySubscriber memorySubscriber(configId, memoryNameSystem());

Create the client navigator, which will allow us to send navigation commands to the navigator component.

armarx::navigation::client::Navigator navigator(armarx::navigation::client::Navigator::InjectedServices{
        .navigator = &iceNavigator, .subscriber = &memorySubscriber});

Now, configure the navigation stack for your client. Start with the general configuration:

armarx::navigation::client::GeneralConfig generalParams{};

For global planning, use the following configuration:

armarx::navigation::global_planning::SPFAParams globalPlannerParams{};

In this example, local planning is not used. For the trajectory controller, configure parameters such as velocity limits:

armarx::navigation::traj_ctrl::local::TrajectoryFollowingControllerParams
        trajControllerParams{};
 
// Example: Restrict the platform's linear velocity
trajControllerParams.limits.linear = 420; // [mm/s]

Combine these configurations into a complete navigation stack configuration:

armarx::navigation::client::NavigationStackConfig cfg;
cfg.general(generalParams);
cfg.globalPlanner(globalPlannerParams);
cfg.trajectoryController(trajControllerParams);

Finally, register this configuration with the navigator server:

iceNavigator.createConfig(cfg, configId);

This step ensures that navigation requests from this client will use the specified configuration. You can verify this by checking the navigation memory (using the MemoryViewer widget), where an entry should now exist in the Navigation/Parameterization core segment with the name of your component.

Sending Navigation Commands

To move the robot, specify the target coordinates. For example, to navigate to the global position [1000, 1000]:

const Eigen::Isometry3f globalTarget(Eigen::Translation3f{1000, 1000, 0});

Send the navigation command:

ARMARX_INFO << "Sending navigation command";
navigator.moveTo(globalTarget, armarx::core::NavigationFrame::Absolute);

The robot will start moving toward the specified target. To wait until the robot reaches its goal, use the following:

ARMARX_INFO << "Waiting until the robot reaches its goal ...";
 
const armarx::navigation::client::StopEvent stopEvent = navigator.waitForStop();
 
ARMARX_INFO << "The robot stopped.";

The StopEvent provides information about whether the goal was reached or if a failure occurred. For example:

if(stopEvent.isGoalReachedEvent())
{
    ARMARX_INFO << "The robot reached its goal";
}
else
{
    ARMARX_WARNING << "Something went wrong.";
}

Congratulations! You have successfully implemented a navigation client that can send commands and handle events. The robot should now navigate to the specified target or provide feedback if an issue arises.

Usage Example

Skills

The available skills, e.g., NavigateTo show the use of the navigator API including failure / event handling. Therefore, this implementation should be consulted and can be copied.

Example Client

An example component shows several use cases of a navigation client. This includes

• Standard: Demonstrates how to configure and utilize the navigator to reach a specified global target.
  
• Complex: Illustrates the use of via-points, enabling the robot to pass through specific intermediate points on its way to the goal pose.
  
• PointToPoint: Explains how the robot can execute direct point-to-point movements.
  
• UpdateNavigator: Highlights the ability to dynamically update the navigation target during execution.
  
• WanderAround: Continuously samples random reachable navigation targets and moves to them in an infinite loop.
  

The use case can be selected through the mode property of the component.

Scenarios