Navigating a Real Robot#

The navigation recipes so far ran in simulation, on plain ROS topics that the simulator happened to publish. A real robot is not that tidy. Its odometry arrives in a vendor protocol, its LiDAR needs a driver started, its velocity commands have to be encoded for its motion controller, and it has moods of its own, a battery, a fall, an obstacle in front of its nose, that the navigation stack should know about. A plugin adapts all of that to the interfaces the stack uses, and this tutorial runs the Kompass stack from Point Navigation on a real quadruped, the DeepRobotics Lite3, through its plugin.

Nothing in the stack changes. What changes is where its inputs come from and where its output goes.

Installing the plugin#

emos plugin install emos-plugin-lite3
emos plugin inspect emos-plugin-lite3

The install builds the plugin and resolves the sensor drivers its manifest declares. inspect prints what the plugin offers: its feedback streams and their keys, its commands, its actions and events, where its sensors sit, and how it maps. That listing is the reference for every binding below. Plugins covers installing and updating.

Attaching it#

from ros_sugar import Launcher
from lite3_plugin import Lite3Plugin

robot = Lite3Plugin()
launcher = Launcher()
launcher.add_plugin(robot)

The plugin carries the robot’s description: a differential-drive model, a box footprint of the Lite3’s size, its velocity and acceleration limits, and its base frame, body. The launcher hands them to every component at bringup, so the recipe sets no launcher.robot and no frames. A recipe that does set them wins over the plugin, and a unit that differs from the defaults is a subclass:

class MyLite3(Lite3Plugin):
    MOTION_HOST_IP = "10.0.0.42"    # the robot on another subnet
    ROBOT_VX_MAX = 0.6              # a lower speed cap for this unit

Localization from the plugin#

The stack needs to know where the robot is, in the map frame. The Lite3 has no localizer of its own, so the plugin provides one: it fuses the leg odometry with the body IMU in a robot_localization filter and publishes the transforms from map through odom to body. The recipe binds that feed as the location of every component, and the plugin starts the filter because something asked for it:

from kompass.ros import Topic

launcher.inputs(location=Topic(name="odometry_filtered", msg_type="Odometry", use_plugin=True))

use_plugin=True means the robot plugin serves the topic, and the name is the feedback key. The Lite3 has two odometry streams, the raw leg odometry under Odometry and the fused one under odometry_filtered, which is why the key matters here. A topic type the plugin has only one of, such as its Twist command, binds by type whatever it is called.

Sensor data#

The local mapper builds its grid from the LiDAR’s point cloud. Binding the cloud is what starts the LiDAR driver: the launcher starts a driver only for feeds a recipe binds, so a recipe that never reads the cloud never runs it.

from kompass.components import LocalMapper, LocalMapperConfig
from kompass.control import MapConfig

local_mapper = LocalMapper(
    component_name="mapper",
    config=LocalMapperConfig(loop_rate=5.0, map_params=MapConfig(width=4.0, height=4.0, resolution=0.1)),
)
local_mapper.inputs(sensor_data=Topic(name="lidar", msg_type="PointCloud2", use_plugin=True))

The LiDAR’s driver publishes the cloud on a ROS topic of its own, so the binding just points the subscriber at it, and the key lidar is the same whichever LiDAR the robot carries. The LiDAR’s frame is placed on the body by the plugin’s mounts, published as a static transform, so the mapper knows where the sensor sits without a URDF. The robot’s other streams bind the same way when a recipe wants them: the RealSense image under camera, the two ultrasound sensors under ultrasound_front and ultrasound_back, the battery, the joint states.

Commands#

The drive manager is the last link before the robot. Its output is a Twist, and bound to the plugin it goes out as the Lite3’s own velocity packets over UDP, with the drive manager’s safety zones still applied before anything is sent:

from kompass.components import DriveManager, DriveManagerConfig

driver = DriveManager(
    component_name="drive_manager",
    config=DriveManagerConfig(critical_zone_distance=0.3, critical_zone_angle=90.0, slowdown_zone_distance=0.6),
)
driver.inputs(
    sensor_data=[
        Topic(name="lidar", msg_type="PointCloud2", use_plugin=True),
        Topic(name="ultrasound_front", msg_type="Range", use_plugin=True),
        Topic(name="ultrasound_back", msg_type="Range", use_plugin=True),
    ]
)
driver.outputs(robot_command=Topic(name="Twist", msg_type="Twist", use_plugin=True))

The safety zones check every sensor they are given and act on the most cautious one. The LiDAR on the robot’s back sees the whole scene; the two ultrasounds, at body height on the nose and the tail, are a second opinion right where the robot is heading. The plugin’s mounts tell the drive manager which way each beam points, so the front one guards forward motion and the rear one reversing, and the movement actions, such as the move_to_unblock recovery below, consult all three before they move.

A Range reading is the distance from the sensor itself, while the LiDAR’s critical zone is measured from the robot’s edge. The Lite3’s ultrasounds read nothing closer than 0.28 m, and a reading outside a sensor’s own limits counts as clear, so the critical distance has to reach at least that far for them to ever stop the robot. 0.3 m does, and the slowdown zone moves out to stay beyond it.

Every safety sensor has to keep publishing. One whose last message is older than sensor_data_timeout, 0.2 s by default, stops the robot until it comes back. When losing one sensor should not stop the robot, stale_sensor_policy="skip" runs the check on the others. Drive Manager covers the safety check in full.

While the recipe runs, the plugin keeps the robot under external control with a heartbeat, and when the recipe ends, however it ends, it hands the robot back to its handset.

The map#

Kompass’s map server needs the site’s map, and the plugin knows which one is active. The map is built with emos map new, which the plugin declares how to do for this robot, and made active with emos map use; see Mapping. The recipe then asks the plugin for it:

from kompass.components import MapServer, MapServerConfig

map_server = MapServer(
    component_name="global_map_server",
    config=MapServerConfig(loop_rate=1.0, map_file_path=robot.MAPPING.active_grid_path()),
)

The rest of the stack#

The planner and the controller are set up exactly as in Point Navigation, and a goal arrives as a clicked point or from the recipe’s web interface:

from kompass.components import Controller, Planner, PlannerConfig
from kompass.control import ControllersID

planner = Planner(component_name="planner", config=PlannerConfig(loop_rate=1.0))
planner.run_type = "ActionServer"
goal = Topic(name="/clicked_point", msg_type="PointStamped")
planner.inputs(goal_point=goal)

controller = Controller(component_name="controller")
controller.algorithm = ControllersID.DWA
controller.direct_sensor = False   # local perception from the mapper's grid

The robot’s own events and actions#

The plugin brings the robot’s vocabulary with it. The Lite3 reports a low battery, an obstacle ahead of its front ultrasound, a disturbed balance and a fall, and its actions are its motion-host behaviours: sit or stand, the three gaits, a stop, and a few tricks. They register like any other event and action, and they sit naturally next to the stack’s own events:

from kompass.ros import Action
from ros_sugar.actions import stop

launcher.on(robot.events.low_battery(15.0), robot.actions.sit_stand())
launcher.on(robot.events.fallen(), [stop(component=controller), Action(method=planner.cancel_main_goal)])

Here a fall stops the controller and cancels the goal the planner is working on, so the robot does not set off again on its own once it is back on its feet.

Build them after the plugin is attached, since attaching binds the identity the events resolve against.

The Lite3 never acknowledges a command, so an action’s result only says the command was sent. Its telemetry says whether anything happened, and keyword arguments given to an action factory go to the Action it builds, so an action can wait for the robot to confirm:

status = robot.feedbacks["robot_status"].as_topic()
stand_up = robot.actions.sit_stand(success=status.msg.data == "standing", timeout=10.0)

This is a monitored action, described in Events and Actions, and it works the same as a step of a routine.

A sensor on top#

A sensor that is not part of the robot is a sensor plugin, attached with a Mount that says where it sits, so its frame exists in TF. A thermal PTZ camera on the Lite3’s back:

from ros_sugar.robot import Mount
from hikmicro_plugin import HikmicroBispectrum

camera = HikmicroBispectrum(id="inspection_cam")
launcher.add_plugin(camera, mount=Mount(parent=robot, xyz=(0.15, 0.0, 0.35)))

thermal = Topic(name="thermal_image", msg_type="Image", use_plugin=camera.id)
launcher.on(camera.events.over_temperature(60.0), camera.actions.goto_preset(1))

A sensor plugin is named by its id in use_plugin, and it comes with events and actions of its own, here the camera’s thermometry and its pan, tilt and presets.

Running it#

emos info shows, for every topic in the recipe, what provides it: the robot plugin, a sensor plugin by its id, or a plain ROS topic. Then run it, and the plugin starts the drivers the recipe bound:

emos info lite3_navigation
emos run lite3_navigation

Complete code#

The Kompass stack on a Lite3#

from automatika_ros_sugar.msg import ComponentStatus
from kompass.components import (
    Controller,
    DriveManager,
    DriveManagerConfig,
    LocalMapper,
    LocalMapperConfig,
    MapServer,
    MapServerConfig,
    Planner,
    PlannerConfig,
    TopicsKeys,
)
from kompass.control import ControllersID, MapConfig
from kompass.ros import Action, Event, Launcher, Topic
from ros_sugar.actions import stop
from lite3_plugin import Lite3Plugin

# --- The robot ---
robot = Lite3Plugin()

# --- The map the robot was mapped with ---
map_server = MapServer(
    component_name="global_map_server",
    config=MapServerConfig(loop_rate=1.0, map_file_path=robot.MAPPING.active_grid_path()),
)

# --- Planning and control, as in Point Navigation ---
planner = Planner(component_name="planner", config=PlannerConfig(loop_rate=1.0))
planner.run_type = "ActionServer"
goal = Topic(name="/clicked_point", msg_type="PointStamped")
planner.inputs(goal_point=goal)

controller = Controller(component_name="controller")
controller.algorithm = ControllersID.DWA
controller.direct_sensor = False

# --- Local perception from the robot's LiDAR ---
local_mapper = LocalMapper(
    component_name="mapper",
    config=LocalMapperConfig(loop_rate=5.0, map_params=MapConfig(width=4.0, height=4.0, resolution=0.1)),
)
local_mapper.inputs(sensor_data=Topic(name="lidar", msg_type="PointCloud2", use_plugin=True))

# --- Commands out through the plugin, guarded by the LiDAR and both ultrasounds ---
driver = DriveManager(
    component_name="drive_manager",
    config=DriveManagerConfig(critical_zone_distance=0.3, critical_zone_angle=90.0, slowdown_zone_distance=0.6),
)
driver.inputs(
    sensor_data=[
        Topic(name="lidar", msg_type="PointCloud2", use_plugin=True),
        Topic(name="ultrasound_front", msg_type="Range", use_plugin=True),
        Topic(name="ultrasound_back", msg_type="Range", use_plugin=True),
    ]
)
driver.outputs(robot_command=Topic(name="Twist", msg_type="Twist", use_plugin=True))

# --- The launcher, with the robot attached ---
launcher = Launcher()
launcher.add_plugin(robot)

# --- Events: the stack's and the robot's ---
emergency_stop = driver.get_out_topic(TopicsKeys.EMERGENCY)
robot_blocked = Event(
    emergency_stop.msg.data.is_true()
    | (controller.status_topic.msg.status == ComponentStatus.STATUS_FAILURE_ALGORITHM_LEVEL)
)
send_goal = Action(
    method=planner.trigger_main_action_server,
    kwargs={"goal_x": goal.msg.point.x, "goal_y": goal.msg.point.y, "tolerance_dist": 0.1, "tolerance_ori": 0.2},
)

launcher.add_pkg(
    components=[map_server, planner, controller, local_mapper, driver],
    events_actions={
        Event(goal): send_goal,
        robot_blocked: Action(method=driver.move_to_unblock),
    },
    package_name="kompass",
    multiprocessing=True,
)
launcher.on(robot.events.low_battery(15.0), robot.actions.sit_stand())
launcher.on(robot.events.fallen(), [stop(component=controller), Action(method=planner.cancel_main_goal)])

# --- Localization from the plugin, for every component ---
odom = Topic(name="odometry_filtered", msg_type="Odometry", use_plugin=True)
launcher.inputs(location=odom)

# --- The web interface: goals in; map, plan and odometry out ---
launcher.enable_ui(
    inputs=[planner.ui_main_action_input],
    outputs=[
        map_server.get_out_topic(TopicsKeys.GLOBAL_MAP),
        odom,
        planner.get_out_topic(TopicsKeys.GLOBAL_PLAN),
    ],
)
launcher.bringup()