SPRESENSEで自動走行ゴミ箱

今回，自動走行機能付きゴミ箱を作成しました．

ゴミが発生したときに，歩くのがめんどくさい時ってありますよね．
ゴミ箱から自分の元に来てくれたらいいのになって思ったりしますよね．
そんな思いを実現するロボットを作りました！

それが……

ここに動画が表示されます

自動走行によって移動できる小型ゴミ箱ロボットです．
自動操作の予約機能を用意して，ゴミ出しを楽したいと考えていました．
しかし，実装に苦労してしまい，ティッシュを運ぶロボットになってしまいました．
とても可愛いという利点があるため，ペットの様になっています．

通信用AP機能付きルータ(無線LAN環境)

スマートフォン

	SLAM用ノートPC
OS	Windows 11
CPU	Intel(R) Core(TM) i7-10875H CPU @ 2.30GHz (16 CPUs), ~2.3GHz
RAM	16384MB
GPU	NVIDIA GeForce RTX 2060
ROS	ROS2 Jazzy Jalisco

	Raspberry Pi 5
OS	Ubuntu24.04
ROS	ROS2 Jazzy Jalisco

SPRESENSE メインボード x2 (走行制御用とToFセンサ用)
SPRESENSE 拡張ボード

SPRESENSE Wi-Fi Add-onボード iS110B

Zumo Robot for Arduino (Assembled with 75:1 HP Motors)
ToFセンサーボード（SPRESENSE用）（MM-TOF10-IS）

FHL-LD19 12Meter Lidar

連結ヘッダソケット

オスメスジャンパー線

モバイルバッテリー

段ボール

microROSを動かしてWiFiルータ経由で制御情報をSubscribeしています．
ZumoにはIMUが搭載されているため，IMU情報のPublish, オドメトリのPublishなども行おうとしているのですが，現状は実装できていません．
Publish実装の際にサブコアへの分離等も行いたいと考えています．

Zumoとの接続時はSPRESENSE拡張ボードを使用しているのですが，Arduino UNOとの互換性が完全ではないため，PWMのピンを繋ぎ変える必要がありました．

したがって，pin10 → pin6, pin9 → pin5となるように繋ぎ変えています．

繋ぎ変えにはジャンパー線を使い，繋ぎ変えない場所は連結ヘッダソケットを用いて本来のマニュアル通りに接続しています．

また，起動時は拡張ボードの全てのデジタル端子がプルアップされるため，制御で使っているPinを全てLOWにするようにしています．以下のライブラリに対してピン変更，出力をLowにするパッチを作成し，適用しています．
pololu/zumo-32u4-arduino-library: Library to help interface with the on-board hardware of the Pololu Zumo 32U4 robot.

Spresense-microROS-Seminar/Sketches/spresense_micro-ros_subscriber_GS2200/spresense_micro-ros_subscriber_GS2200.ino at main · TE-YoshinoriOota/Spresense-microROS-Seminar
を参考に実装しています．


メインプログラム
#include "wifi.h"
#include <ZumoShield.h>
#include <micro_ros_arduino.h>
#include <geometry_msgs/msg/twist.h>
#include <rcl/error_handling.h>
#include <rcl/rcl.h>
#include <rclc/executor.h>
#include <rclc/rclc.h>

static rcl_allocator_t allocator;
static rclc_support_t support;
static rcl_node_t node;
static rcl_subscription_t subscriber;
static rclc_executor_t executor;
static rcl_timer_t timer;
static geometry_msgs__msg__Twist received_twist;

ZumoMotors motors;

#define RCCHECK(fn)                    \
    {                                  \
        rcl_ret_t temp_rc = fn;        \
        if ((temp_rc != RCL_RET_OK)) { \
            error_loop();              \
        }                              \
    }
#define RCSOFTCHECK(fn)                \
    {                                  \
        rcl_ret_t temp_rc = fn;        \
        if ((temp_rc != RCL_RET_OK)) { \
        }                              \
    }

void error_loop() {
    motors.setLeftSpeed(0);
    motors.setRightSpeed(0);
    while (1) {
        digitalWrite(LED0, !digitalRead(LED0));
        delay(100);
    }
}

#define LINEAR_SCALE 1.5
#define ANGULAR_SCALE 0.55

void control_motors(float linear, float angular) {
    int leftSpeed = 0;
    int rightSpeed = 0;

    leftSpeed = (int)constrain(
        (linear * LINEAR_SCALE - angular * ANGULAR_SCALE) * 400, -400, 400);
    rightSpeed = (int)constrain(
        (linear * LINEAR_SCALE + angular * ANGULAR_SCALE) * 400, -400, 400);

#ifdef WIFI_H_
    Serial.printf("leftSpeed: %d, rightSpeed: %d\n", leftSpeed, rightSpeed);
#endif

    motors.setLeftSpeed(leftSpeed);
    motors.setRightSpeed(rightSpeed);
}

void twist_callback(const void* msgin) {
    const geometry_msgs__msg__Twist* twist_msg =
        (const geometry_msgs__msg__Twist*)msgin;

    float linear_x = twist_msg->linear.x;
    float angular_z = twist_msg->angular.z;

    digitalWrite(LED3, (twist_msg->linear.x > 0) ? HIGH : LOW);
#ifdef WIFI_H_
    Serial.printf("linear_x: %f, angular_z: %f\n", linear_x, angular_z);
#endif
    control_motors(linear_x, angular_z);
}

void setup() {
#ifdef WIFI_H_
    Serial.begin(115200);
    Serial.println("Start");
    rmw_uros_set_custom_transport(
        false, NULL, arduino_wifi_transport_open, arduino_wifi_transport_close,
        arduino_wifi_transport_write, arduino_wifi_transport_read);
#else  // _WIFI_CPP
    set_microros_transports();
#endif

    motors.setLeftSpeed(0);
    motors.setRightSpeed(0);

    allocator = rcl_get_default_allocator();

    // create init_options
    RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

    // create node
    RCCHECK(
        rclc_node_init_default(&node, "arduino_twist_listener", "", &support));

    // create subscriber
    RCCHECK(rclc_subscription_init_default(
                &subscriber, &node,
                ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
                "cmd_vel_nav");)

    // create executor
    RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));

    RCCHECK(rclc_executor_add_subscription(
        &executor, &subscriber, &received_twist, &twist_callback, ON_NEW_DATA));
    digitalWrite(LED3, HIGH);
}

void loop() {
    delay(100);
    RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}


WiFi通信用プログラム(wifi.cpp)
#include "wifi.h"

ATCMD_NetworkStatus net_status;
extern uint8_t ESCBuffer[];
extern uint32_t ESCBufferCnt;
const char server_ip[16] = SERVER_IP;
const char server_port[6] = SERVER_PORT;
const char client_port[6] = "10001";
uint8_t client_id = 0;

extern "C" {

bool arduino_wifi_transport_open(struct uxrCustomTransport* transport) {
    ATCMD_REGDOMAIN_E regDomain;
    const char ssid[32] = AP_SSID;
    const char pswd[32] = AP_PASSPHRASE;
    char macid[20];

    // Initiallize SPI
    Init_GS2200_SPI_type(iS110B_TypeC);

    // check the io
    while (Get_GPIO37Status()) {
        if (AtCmd_RecvResponse() == ATCMD_RESP_NORMAL_BOOT_MSG)
            ConsoleLog("Normal Boot\r\n");
    }

    // setup
    ATCMD_CHECK(AtCmd_AT());
    ATCMD_CHECK(AtCmd_ATE(0));
    ATCMD_CHECK(AtCmd_WREGDOMAIN_Q(&regDomain));
    if (regDomain != ATCMD_REGDOMAIN_TELEC) {
        ATCMD_CHECK(AtCmd_WREGDOMAIN(ATCMD_REGDOMAIN_TELEC));
    }
    ATCMD_CHECK(AtCmd_NMAC_Q(macid));
    ATCMD_CHECK(AtCmd_VER());
    ATCMD_CHECK(AtCmd_WRXACTIVE(0));
    ATCMD_CHECK(AtCmd_WRXPS(1));
    ATCMD_CHECK(AtCmd_BDATA(1));

    // connect to your access point
    ATCMD_CHECK(AtCmd_WM(ATCMD_MODE_STATION));
    ATCMD_CHECK(AtCmd_WD());
    ATCMD_CHECK(AtCmd_NDHCP(1));
    ATCMD_CHECK(AtCmd_WPAPSK(ssid, pswd));
    ATCMD_CHECK(AtCmd_WA(ssid, "", 0));
    ATCMD_CHECK(AtCmd_WSTATUS());
    Serial.println("WiFi connected");

    while (true) {
        ATCMD_RESP_E r;
        r = AtCmd_NCUDP(server_ip, server_port, client_port, &client_id);
        if (r != ATCMD_RESP_OK) {
            ConsoleLog("No connect. Retry\n");
            delay(1000);
            continue;
        }

        if (client_id == ATCMD_INVALID_CID) {
            ConsoleLog("Got an invalid client id. Retry\n");
            delay(2000);
            continue;
        }
        ConsolePrintf("client_id: %d\r\n", client_id);

        ATCMD_CHECK(AtCmd_NSTAT(&net_status));
        ConsoleLog("Connected to the server\n");
        ConsolePrintf("IP: %d.%d.%d.%d\n", net_status.addr.ipv4[0],
                      net_status.addr.ipv4[1], net_status.addr.ipv4[2],
                      net_status.addr.ipv4[3]);
        break;
    }
    return true;
}

bool arduino_wifi_transport_close(struct uxrCustomTransport* transport) {
    /* if this enables, the program will crash when the ros2 agent is not
     * running on the network */
    /* Since micro-ROS calls the close-API and the write-API simultaneously, */
    /* the write-API may cause the probelm during closing of the network (timing
     * issue)         */
    return true;
}

size_t arduino_wifi_transport_write(struct uxrCustomTransport* transport,
                                    const uint8_t* buf,
                                    size_t len,
                                    uint8_t* errcode) {
    (void)errcode;
#ifdef DEBUG
    Serial.println("arduino_wifi_transport_write");
    for (int n = 0; n < len; ++n) {
        Serial.print(buf[n], HEX);
        Serial.print(" ");
    }
    Serial.println();
#endif

    Serial.println("Start to send udp data: " + String(len) + " Bytes");
    if (len == 0)
        return len;

    WiFi_InitESCBuffer();
    // while (AtCmd_UDP_SendBulkData(client_id, buf, len, net_status.addr.ipv4,
    // client_port) != ATCMD_RESP_OK) {
    if (AtCmd_SendBulkData(client_id, buf, len) != ATCMD_RESP_OK) {
        Serial.println("fails to send");
        return 0;
    }
    return len;
}

size_t arduino_wifi_transport_read(struct uxrCustomTransport* transport,
                                   uint8_t* buf,
                                   size_t len,
                                   int timeout,
                                   uint8_t* errcode) {
    (void)errcode;
    int res = 0;
    uint32_t start_time = millis();
    if (AtCmd_RecvResponse() == ATCMD_RESP_BULK_DATA_RX) {
        if (Check_CID(client_id)) {
            ConsolePrintf("Receive %d bytes\r\n", ESCBufferCnt - 1,
                          ESCBuffer + 1);
            memcpy(buf, ESCBuffer + 1, ESCBufferCnt - 1);
            res = ESCBufferCnt - 1;
        } else {
            Serial.println("clinet_id is not match: " + String(client_id));
        }
        WiFi_InitESCBuffer();
    }
#ifdef DEBUG
    Serial.println("arduino_wifi_transport_read: " + String(res));
    for (int n = 0; n < res; ++n) {
        Serial.print(buf[n], HEX);
        Serial.print(" ");
    }
    Serial.println();
#endif
    return res;
}
}


WiFi通信用ヘッダファイル(wifi.h)
#ifndef WIFI_H_
#define WIFI_H_

#define ATCMD_CHECK(fn)             \
    {                               \
        while (fn != ATCMD_RESP_OK) \
            ;                       \
    }

#include <micro_ros_arduino.h>
#include <uxr/client/transport.h>
#include <rmw_microros/rmw_microros.h>

#include <GS2200AtCmd.h>
#include <GS2200Hal.h>
#include "config.h"

extern ATCMD_NetworkStatus net_status;
extern uint8_t ESCBuffer[];
extern uint32_t ESCBufferCnt;
extern const char server_ip[16];
extern const char server_port[6];
extern const char client_port[6];
extern uint8_t client_id;

#ifdef __cplusplus
extern "C" {
#endif

bool arduino_wifi_transport_open(struct uxrCustomTransport* transport);
bool arduino_wifi_transport_close(struct uxrCustomTransport* transport);
size_t arduino_wifi_transport_write(struct uxrCustomTransport* transport,
                                    const uint8_t* buf,
                                    size_t len,
                                    uint8_t* errcode);
size_t arduino_wifi_transport_read(struct uxrCustomTransport* transport,
                                   uint8_t* buf,
                                   size_t len,
                                   int timeout,
                                   uint8_t* errcode);

#ifdef __cplusplus
}
#endif

#endif  // WIFI_H_

ToFセンサーボード（SPRESENSE用）（MM-TOF10-IS）とSPRESENSE Wi-Fi Add-onボード iS110Bの同時接続が出来ないため，2台目のSPRESENSEを用意しました．
当初はSLAMに使用する予定だったのですが，LiDARを購入したため，内容量の測定に使用することにしました．

Raspberry Pi 5とシリアル通信を行うことでWiFiとの同時接続をせずにROS2のネットワークに加えようと考えているのですが，LAN全体には情報がPublishされていないため，内容量情報の制御をRaspberry Pi 5でどのように行うべきか検討しています．

Raspberry Pi 5は主に，FHL-LD19 12Meter Lidar
の情報をROS2に変換することと，制御用SPRESENSEへの電源供給を担っています．

Ubuntu 24.04をインストールし，ROS2 Jazzy Jaliscoをインストールしています．
電源供給にモバイルバッテリーを接続しています．

以下のライブラリをビルドし，LiDARの情報をPublishし，SLAMに利用しています．
ビルド時にJazzy Jaliscoではビルドできないよう設定されているため，パッチを作成して回避しています．互換性が保証されないため，あまりおすすめはしません……．現状は特に問題なく動いています．

Myzhar/ldrobot-lidar-ros2: ROS2 package for LDRobot lidar. Based on ROS2 Lifecycle nodes

SLAM用PCでは，slam_toolbox，Nav2，操作・動作確認用のrviz2を実行しています．

rviz2は
ros2 run rviz2 rviz2 -d $(ros2 pkg prefix nav2_bringup)/share/nav2_bringup/rviz/nav2_default_view.rviz scan:=ldlidar /scan
で実行することで，Nav2のゴール設定をrviz2経由で楽に設定できるようにしています．

LiDARでのslam_toolboxのための設定はMyzhar/ldrobot-lidar-ros2: ROS2 package for LDRobot lidar. Based on ROS2 Lifecycle nodesを元にしています．


amcl:
  ros__parameters:
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "ldlidar_base"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 100.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 2000
    min_particles: 500
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.2
    update_min_d: 0.25
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    scan_topic: scan

bt_navigator:
  ros__parameters:
    global_frame: map
    robot_base_frame: ldlidar_base
    odom_topic: odom
    bt_loop_duration: 10
    default_server_timeout: 20
    wait_for_service_timeout: 1000
    action_server_result_timeout: 900.0
    navigators: ["navigate_to_pose", "navigate_through_poses"]
    navigate_to_pose:
      plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
    navigate_through_poses:
      plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.

    # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings).
    # Built-in plugins are added automatically
    # plugin_lib_names: []

    error_code_names:
      - compute_path_error_code
      - follow_path_error_code

controller_server:
  ros__parameters:
    controller_frequency: 5.0
    costmap_update_timeout: 0.30
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugins: ["progress_checker"]
    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
    controller_plugins: ["FollowPath"]
    use_realtime_priority: false

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    # Goal checker parameters
    #precise_goal_checker:
    #  plugin: "nav2_controller::SimpleGoalChecker"
    #  xy_goal_tolerance: 0.25
    #  yaw_goal_tolerance: 0.25
    #  stateful: True
    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      time_steps: 56
      model_dt: 0.2
      batch_size: 2000
      ax_max: 3.0
      ax_min: -3.0
      ay_max: 3.0
      az_max: 3.5
      vx_std: 0.2
      vy_std: 0.2
      wz_std: 0.4
      vx_max: 0.5
      vx_min: -0.35
      vy_max: 0.5
      wz_max: 1.9
      iteration_count: 1
      prune_distance: 1.7
      transform_tolerance: 0.1
      temperature: 0.3
      gamma: 0.015
      motion_model: "DiffDrive"
      visualize: true
      regenerate_noises: true
      TrajectoryVisualizer:
        trajectory_step: 5
        time_step: 3
      AckermannConstraints:
        min_turning_r: 0.2
      critics: [
        "ConstraintCritic", "CostCritic", "GoalCritic",
        "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
        "PathAngleCritic", "PreferForwardCritic"]
      ConstraintCritic:
        enabled: true
        cost_power: 1
        cost_weight: 4.0
      GoalCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 1.4
      GoalAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.0
        threshold_to_consider: 0.5
      PreferForwardCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 0.5
      CostCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.81
        critical_cost: 300.0
        consider_footprint: true
        collision_cost: 1000000.0
        near_goal_distance: 1.0
        trajectory_point_step: 2
      PathAlignCritic:
        enabled: true
        cost_power: 1
        cost_weight: 14.0
        max_path_occupancy_ratio: 0.05
        trajectory_point_step: 4
        threshold_to_consider: 0.5
        offset_from_furthest: 20
        use_path_orientations: false
      PathFollowCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        offset_from_furthest: 5
        threshold_to_consider: 1.4
      PathAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 2.0
        offset_from_furthest: 4
        threshold_to_consider: 0.5
        max_angle_to_furthest: 1.0
        mode: 0
      # TwirlingCritic:
      #   enabled: true
      #   twirling_cost_power: 1
      #   twirling_cost_weight: 10.0

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: ldlidar_base
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.22
      plugins: ["voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.70
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          # A relative topic will be appended to the parent of the local_costmap namespace.
          # For example:
          #   * User chosen namespace is `tb4`.
          #   * User chosen topic is `scan`.
          #   * Topic will be remapped to `/tb4/scan` without `local_costmap`.
          #   * Use global topic `/scan` if you do not wish the node namespace to apply
          topic: scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      always_send_full_costmap: True

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: ldlidar_base
      robot_radius: 0.22
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          # A relative topic will be appended to the parent of the global_costmap namespace.
          # For example:
          #   * User chosen namespace is `tb4`.
          #   * User chosen topic is `scan`.
          #   * Topic will be remapped to `/tb4/scan` without `global_costmap`.
          #   * Use global topic `/scan` if you do not wish the node namespace to apply
          topic: scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.7
      always_send_full_costmap: True

# The yaml_filename does not need to be specified since it going to be set by defaults in launch.
# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py
# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used.
# map_server:
#   ros__parameters:
#     yaml_filename: ""

map_saver:
  ros__parameters:
    save_map_timeout: 5.0
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: True

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    planner_plugins: ["GridBased"]
    costmap_update_timeout: 1.0
    GridBased:
      plugin: "nav2_navfn_planner::NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

smoother_server:
  ros__parameters:
    smoother_plugins: ["simple_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      max_its: 1000
      do_refinement: True

behavior_server:
  ros__parameters:
    local_costmap_topic: local_costmap/costmap_raw
    global_costmap_topic: global_costmap/costmap_raw
    local_footprint_topic: local_costmap/published_footprint
    global_footprint_topic: global_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
    spin:
      plugin: "nav2_behaviors::Spin"
    backup:
      plugin: "nav2_behaviors::BackUp"
    drive_on_heading:
      plugin: "nav2_behaviors::DriveOnHeading"
    wait:
      plugin: "nav2_behaviors::Wait"
    assisted_teleop:
      plugin: "nav2_behaviors::AssistedTeleop"
    local_frame: odom
    global_frame: map
    robot_base_frame: ldlidar_base
    transform_tolerance: 0.1
    simulate_ahead_time: 2.0
    max_rotational_vel: 0.7
    min_rotational_vel: 0.3
    rotational_acc_lim: 3.2

waypoint_follower:
  ros__parameters:
    loop_rate: 20
    stop_on_failure: false
    action_server_result_timeout: 900.0
    waypoint_task_executor_plugin: "wait_at_waypoint"
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: True
      waypoint_pause_duration: 200

velocity_smoother:
  ros__parameters:
    smoothing_frequency: 20.0
    scale_velocities: False
    feedback: "OPEN_LOOP"
    max_velocity: [0.5, 0.0, 2.0]
    min_velocity: [-0.5, 0.0, -2.0]
    max_accel: [2.5, 0.0, 3.2]
    max_decel: [-2.5, 0.0, -3.2]
    odom_topic: "odom"
    odom_duration: 0.1
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0

collision_monitor:
  ros__parameters:
    base_frame_id: "ldlidar_base"
    odom_frame_id: "odom"
    cmd_vel_in_topic: "cmd_vel_smoothed"
    cmd_vel_out_topic: "cmd_vel"
    state_topic: "collision_monitor_state"
    transform_tolerance: 0.2
    source_timeout: 1.0
    base_shift_correction: True
    stop_pub_timeout: 2.0
    # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
    # and robot footprint for "approach" action type.
    polygons: ["FootprintApproach"]
    FootprintApproach:
      type: "polygon"
      action_type: "approach"
      footprint_topic: "local_costmap/published_footprint"
      time_before_collision: 1.2
      simulation_time_step: 0.1
      min_points: 6
      visualize: False
      enabled: True
    observation_sources: ["scan"]
    scan:
      type: "scan"
      topic: "scan"
      min_height: 0.15
      max_height: 2.0
      enabled: True

docking_server:
  ros__parameters:
    controller_frequency: 50.0
    initial_perception_timeout: 5.0
    wait_charge_timeout: 5.0
    dock_approach_timeout: 30.0
    undock_linear_tolerance: 0.05
    undock_angular_tolerance: 0.1
    max_retries: 3
    base_frame: "ldlidar_base"
    fixed_frame: "odom"
    dock_backwards: false
    dock_prestaging_tolerance: 0.5

    # Types of docks
    dock_plugins: ['simple_charging_dock']
    simple_charging_dock:
      plugin: 'opennav_docking::SimpleChargingDock'
      docking_threshold: 0.05
      staging_x_offset: -0.7
      use_external_detection_pose: true
      use_battery_status: false # true
      use_stall_detection: false # true

      external_detection_timeout: 1.0
      external_detection_translation_x: -0.18
      external_detection_translation_y: 0.0
      external_detection_rotation_roll: -1.57
      external_detection_rotation_pitch: -1.57
      external_detection_rotation_yaw: 0.0
      filter_coef: 0.1

    # Dock instances
    # The following example illustrates configuring dock instances.
    # docks: ['home_dock']  # Input your docks here
    # home_dock:
    #   type: 'simple_charging_dock'
    #   frame: map
    #   pose: [0.0, 0.0, 0.0]

    controller:
      k_phi: 3.0
      k_delta: 2.0
      v_linear_min: 0.15
      v_linear_max: 0.15
      use_collision_detection: true
      costmap_topic: "/local_costmap/costmap_raw"
      footprint_topic: "/local_costmap/published_footprint"
      transform_tolerance: 0.1
      projection_time: 5.0
      simulation_step: 0.1
      dock_collision_threshold: 0.3

loopback_simulator:
  ros__parameters:
    base_frame_id: "ldlidar_base"
    odom_frame_id: "odom"
    map_frame_id: "map"
    scan_frame_id: "base_scan"  # tb4_loopback_simulator.launch.py remaps to 'rplidar_link'
    update_duration: 0.02

slam_toolboxのパラメータ

slam_toolbox: ros__parameters: # Plugin params solver_plugin: solver_plugins::CeresSolver ceres_linear_solver: SPARSE_NORMAL_CHOLESKY ceres_preconditioner: SCHUR_JACOBI ceres_trust_strategy: LEVENBERG_MARQUARDT ceres_dogleg_type: TRADITIONAL_DOGLEG ceres_loss_function: None # ROS Parameters odom_frame: odom map_frame: map base_frame: ldlidar_base scan_topic: /scan use_map_saver: true mode: mapping #localization # if you'd like to immediately start continuing a map at a given pose # or at the dock, but they are mutually exclusive, if pose is given # will use pose #map_file_name: test_map # map_start_pose: [0.0, 0.0, 0.0] #map_start_at_dock: true debug_logging: false throttle_scans: 1 transform_publish_period: 0.02 #if 0 never publishes odometry map_update_interval: 3.0 resolution: 0.025 max_laser_range: 25.0 # for rastering images minimum_time_interval: 0.5 transform_timeout: 0.2 tf_buffer_duration: 30. stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps enable_interactive_mode: true # General Parameters use_scan_matching: true use_scan_barycenter: true minimum_travel_distance: 0.0 minimum_travel_heading: 0.0 scan_buffer_size: 10 scan_buffer_maximum_scan_distance: 25.0 link_match_minimum_response_fine: 0.1 link_scan_maximum_distance: 1.5 loop_search_maximum_distance: 3.0 do_loop_closing: true loop_match_minimum_chain_size: 10 loop_match_maximum_variance_coarse: 3.0 loop_match_minimum_response_coarse: 0.35 loop_match_minimum_response_fine: 0.45 # Correlation Parameters - Correlation Parameters correlation_search_space_dimension: 0.5 correlation_search_space_resolution: 0.01 correlation_search_space_smear_deviation: 0.1 # Correlation Parameters - Loop Closure Parameters loop_search_space_dimension: 8.0 loop_search_space_resolution: 0.05 loop_search_space_smear_deviation: 0.03 # Scan Matcher Parameters distance_variance_penalty: 0.5 angle_variance_penalty: 1.0 fine_search_angle_offset: 0.00349 coarse_search_angle_offset: 0.349 coarse_angle_resolution: 0.0349 minimum_angle_penalty: 0.9 minimum_distance_penalty: 0.5 use_response_expansion: true

おまけですが，えにぽるを手軽に操作するためのスマホアプリを開発しています．
実際はゴールの指定をスマホから行う機構などが不十分なため，別物の何か状態です．
呼び出しコマンドを送信するだけでなく，内容量の受信を行って表示したり，呼出コマンドを予約することで，ゴミ出しの簡略化などを行おうとしています．