How to know if the plugin has been written properly and loaded into the system successfully?

Course Support ROS2 Navigation Advanced
1

Based on the knowledge I learned from the course. I wrote a plugin to serve a virtual wall based on ROS1. But I don’t really understand the algorithm inside it. I want to know if there is a way to check whether the plugin is loaded into the system or not and what order to place it in the available layers. Can someone help me? I can provide the code below:

amcl:
  ros__parameters:
    use_sim_time: False
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "base_footprint"
    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

    # Set init pose Localiztion

    set_initial_pose: true
    initial_pose: 
      x: 0.336452
      y: -0.0605629
      yaw: 0.00679869

amcl_map_client:
  ros__parameters:
    use_sim_time: False

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: False


# Trung gian giua Global plan va local plan

bt_navigator:
  ros__parameters:
    use_sim_time: False
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
    bt_loop_duration: 10
    default_server_timeout: 20
    enable_groot_monitoring: True
    groot_zmq_publisher_port: 1666
    groot_zmq_server_port: 1667
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_compute_path_through_poses_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_clear_costmap_service_bt_node
    - nav2_is_stuck_condition_bt_node
    - nav2_goal_reached_condition_bt_node
    - nav2_goal_updated_condition_bt_node
    - nav2_initial_pose_received_condition_bt_node
    - nav2_reinitialize_global_localization_service_bt_node
    - nav2_rate_controller_bt_node
    - nav2_distance_controller_bt_node
    - nav2_speed_controller_bt_node
    - nav2_truncate_path_action_bt_node
    - nav2_goal_updater_node_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node
    - nav2_round_robin_node_bt_node
    - nav2_transform_available_condition_bt_node
    - nav2_time_expired_condition_bt_node
    - nav2_distance_traveled_condition_bt_node
    - nav2_single_trigger_bt_node
    - nav2_is_battery_low_condition_bt_node
    - nav2_navigate_through_poses_action_bt_node
    - nav2_navigate_to_pose_action_bt_node
    - nav2_remove_passed_goals_action_bt_node
    - nav2_planner_selector_bt_node
    - nav2_controller_selector_bt_node
    - nav2_goal_checker_selector_bt_node

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: False

controller_server:
  ros__parameters:
    use_sim_time: False
    controller_frequency: 10.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["general_goal_checker"] 
    controller_plugins: ["FollowPath"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    
    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25

    # DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: 0.0
      min_vel_y: 0.0
      max_vel_x: 0.22
      max_vel_y: 0.0
      max_vel_theta: 1.0
      min_speed_xy: 0.0
      max_speed_xy: 0.22
      min_speed_theta: 0.0
      # Add high threshold velocity for turtlebot 3 issue.
      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
      acc_lim_x: 2.5
      acc_lim_y: 0.0
      acc_lim_theta: 3.2
      decel_lim_x: -2.5
      decel_lim_y: 0.0
      decel_lim_theta: -3.2
      vx_samples: 20
      vy_samples: 0
      vtheta_samples: 40
      sim_time: 1.5
      linear_granularity: 0.05
      angular_granularity: 0.025
      transform_tolerance: 0.2
      xy_goal_tolerance: 0.05
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: True
      stateful: True
      # critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
      critics: ["RotateToGoal", "Oscillation", "ObstacleFootprint", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
      ObstacleFootprint.scale: 0.02
      PathAlign.scale: 32.0
      PathAlign.forward_point_distance: 0.1
      GoalAlign.scale: 24.0
      GoalAlign.forward_point_distance: 0.1
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      RotateToGoal.scale: 32.0
      RotateToGoal.slowing_factor: 5.0
      RotateToGoal.lookahead_time: -1.0
      
controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: False
      rolling_window: true
      # Size of Costmap area around the robot 
      width: 3
      height: 3
      resolution: 0.05

      # robot_radius: 0.1
      footprint: '[ [0.089, 0.069], [0.089, -0.069], [-0.089, -0.069], [-0.089, 0.069] ]'

      # plugins: ["obstacle_layer", "voxel_layer", "inflation_layer", "virtual_wall"]
      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]

      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        inflation_radius: 1.0
        cost_scaling_factor: 3.0
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      # virtual_wall:
      #   # plugin: virtual_wall/VirtualWall
      #   plugin: virtual_wall/VirtualWall"
      #   enabled: True
      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:
          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:
        map_subscribe_transient_local: True
      always_send_full_costmap: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: False
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      robot_radius: 0.1
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer","virtual_wall"]
      # plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"]

      virtual_wall:
        # plugin: virtual_wall/VirtualWall
        plugin: virtual_wall/VirtualWall"
        enabled: True
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          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
      # Pointcloud Data
      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:
          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.55
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: False
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

map_server:
  ros__parameters:
    use_sim_time: False
    yaml_filename: "map.yaml"

map_saver:
  ros__parameters:
    use_sim_time: False
    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
    use_sim_time: False
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: False

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

recoveries_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    recovery_plugins: ["spin", "backup", "wait"]
    spin:
      plugin: "nav2_recoveries/Spin"
    backup:
      plugin: "nav2_recoveries/BackUp"
    wait:
      plugin: "nav2_recoveries/Wait"
    global_frame: odom
    robot_base_frame: base_link
    transform_timeout: 0.1
    use_sim_time: true
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

robot_state_publisher:
  ros__parameters:
    use_sim_time: False

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

I have configured like that, when loading Rviz, it displays the results:

image
image979×707 46.6 KB

2

No matter where I put the virtual_wall config, the program cannot load the costmap. Have I written the wrong plugin cpp file somewhere? Please help me:

#include <pluginlib/class_list_macros.hpp>
#include <virtual_wall/virtual_wall.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include "nav2_costmap_2d/costmap_math.hpp"

PLUGINLIB_EXPORT_CLASS(virtual_wall::VirtualWall, nav2_costmap_2d::Layer)

using namespace std;

namespace virtual_wall
{
    VirtualWall::VirtualWall()
    {
        cout << "VirtualWall()" << endl;
        wallmax_x_ = 0.0;
        wallmax_y_ = 0.0;
        wallmin_x_ = 0.0;
        wallmin_y_ = 0.0;
    }
    VirtualWall::~VirtualWall()
    {
        cout << "~VirtualWall()" << endl;
    }

    void VirtualWall::onInitialize()
    {
        std::lock_guard<std::recursive_mutex> lock(data_access_);
        // Kiểm tra xem node_ có hợp lệ không trước khi sử dụng
        auto node = node_.lock();
        declareParameter("enabled", rclcpp::ParameterValue(true));
        node->get_parameter(name_ + "." + "enabled", enabled_);
        if (!node)
        {
            // Xử lý trường hợp node đã bị hủy
            std::cerr << "Error: Node has been destroyed." << std::endl;
            return;
        }
        rclcpp::Node::SharedPtr g_nh = std::make_shared<rclcpp::Node>("virtual_wall_" + name_);

        // nh_ = g_nh->create_sub_node("virtual_wall_" + name_);
        matchSize();
        current_ = true;
        // enabled_ = true;

        // Nhaanj du lieu kieu Polygon tu topic /clicked_point
        add_wall_sub_ = g_nh->create_subscription<geometry_msgs::msg::Polygon>(
            "/clicked_point", 1, std::bind(&VirtualWall::AddWallCallback, this, std::placeholders::_1));
        // pub msg Int den topic /virtual_wall_list
        wall_list_pub_ = g_nh->create_publisher<std_msgs::msg::Int32>("/virtual_wall_list", 1);
        delete_wall_sub_ = g_nh->create_subscription<std_msgs::msg::Int32>("/delete_wall", 1, std::bind(&VirtualWall::DeleteWallCallback, this, std::placeholders::_1));
        // pub cac marker len Rviz
        wall_maker_pub_ = g_nh->create_publisher<visualization_msgs::msg::Marker>("virtual_wall_vis", 1);
    }

    void VirtualWall::matchSize()
    {
        std::lock_guard<std::recursive_mutex> lock(data_access_);
        nav2_costmap_2d::Costmap2D *master = layered_costmap_->getCostmap();
        resolution_ = master->getResolution();
        global_frame_ = layered_costmap_->getGlobalFrameID();
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "global_frame_: %s", global_frame_.c_str());
        map_frame_ = "map";
    }

    void VirtualWall::updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y,
                                   double *max_x, double *max_y)
    {
        // useExtraBounds(min_x, min_y, max_x, max_y);
        *min_x = std::min(wallmin_x_, *min_x);
        *min_y = std::min(wallmin_y_, *min_y);
        *max_x = std::max(wallmax_x_, *max_x);
        *max_y = std::max(wallmax_y_, *max_y);
    }

    /* *********************************************************************
     * updateCosts
     *
     * updates the master grid in the area defined in updateBounds
     */

    void VirtualWall::updateCosts(nav2_costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
    {
        std::lock_guard<std::recursive_mutex> lock(data_access_);

        if (global_frame_ == map_frame_)
        {
            visualization_msgs::msg::Marker node_vis;
            node_vis.header.frame_id = map_frame_;

            node_vis.header.stamp = rclcpp::Clock().now();
            node_vis.type = visualization_msgs::msg::Marker::CUBE_LIST;
            node_vis.action = visualization_msgs::msg::Marker::ADD;
            node_vis.id = 0;
            node_vis.pose.orientation.x = 0.0;
            node_vis.pose.orientation.y = 0.0;
            node_vis.pose.orientation.z = 0.0;
            node_vis.pose.orientation.w = 1.0;
            node_vis.color.a = 1.0;
            node_vis.color.r = 1.0;
            node_vis.color.g = 0.0;
            node_vis.color.b = 0.0;

            node_vis.scale.x = resolution_;
            node_vis.scale.y = resolution_;
            node_vis.scale.z = 0.3;

            geometry_msgs::msg::Point pt;
            pt.z = 0.15;

            for (size_t i = 0; i < wallPoint_.size(); i++)
            {
                std_msgs::msg::Int32 msg;
                msg.data = wallPoint_[i].id;
                wall_list_pub_->publish(msg);

                for (size_t j = 0; j < wallPoint_[i].polygon.points.size(); j++)
                {
                    unsigned int pixle_x;
                    unsigned int pixle_y;
                    bool ret = master_grid.worldToMap(wallPoint_[i].polygon.points[j].x, wallPoint_[i].polygon.points[j].y, pixle_x, pixle_y);

                    if (ret)
                    {
                        master_grid.setCost(pixle_x, pixle_y, nav2_costmap_2d::LETHAL_OBSTACLE);
                        pt.x = wallPoint_[i].polygon.points[j].x;
                        pt.y = wallPoint_[i].polygon.points[j].y;
                        node_vis.points.push_back(pt);
                    }
                }
            }
            wall_maker_pub_->publish(node_vis);
        }

        else
        {
            geometry_msgs::msg::TransformStamped transform;
            // tf2_ros::Buffer tf_buffer_;
            try
            {
                /*----------------------------------------------------------------- Chua Transform -----------------------------------------------------------------------*/
                // transform = tf_buffer_->lookupTransform(global_frame_, map_frame_, rclcpp::Time(0));
            }
            catch (tf2::TransformException &ex)
            {
                RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "%s", ex.what());
                return;
            }
            // Copy map data given proper transformations
            tf2::Transform tf2_transform;
            tf2::fromMsg(transform.transform, tf2_transform);

            for (size_t i = 0; i < wallPoint_.size(); ++i)
            {
                std_msgs::msg::Int32 msg;
                msg.data = wallPoint_[i].id;
                wall_list_pub_->publish(msg); // Assuming wall_list_pub is a publisher pointer
                for (size_t j = 0; j < wallPoint_[i].polygon.points.size(); ++j)
                {
                    unsigned int pixle_x;
                    unsigned int pixle_y;
                    double wx, wy;

                    wx = wallPoint_[i].polygon.points[j].x;
                    wy = wallPoint_[i].polygon.points[j].y;
                    tf2::Vector3 p(wx, wy, 0);
                    p = tf2_transform * p;

                    bool ret = master_grid.worldToMap(p.x(), p.y(), pixle_x, pixle_y);
                    if (ret)
                    {
                        master_grid.setCost(pixle_x, pixle_y, nav2_costmap_2d::LETHAL_OBSTACLE);
                    }
                }
            }
        }
    }
    bool VirtualWall::WallInterpolation()
    {
        double pixle_x[2];
        double pixle_y[2];
        double k, b;

        for (size_t i = 0; i < 2; ++i)
        {
            if (fabs(wallPoint_.back().polygon.points[0].x - wallPoint_.back().polygon.points[1].x) > fabs(wallPoint_.back().polygon.points[0].y - wallPoint_.back().polygon.points[1].y))
            {
                pixle_x[i] = wallPoint_.back().polygon.points[i].x;
                pixle_y[i] = wallPoint_.back().polygon.points[i].y;
            }
            else
            {
                pixle_x[i] = wallPoint_.back().polygon.points[i].y;
                pixle_y[i] = wallPoint_.back().polygon.points[i].x;
            }
        }

        // Sử dụng pixle_x và pixle_y ở đây cho mục đích tiếp theo
        k = (pixle_y[0] - pixle_y[1]) / (pixle_x[0] - pixle_x[1]);
        b = pixle_y[0] - k * pixle_x[0];
        wallPoint_.back().polygon.points.clear();

        for (double i = std::min(pixle_x[0], pixle_x[1]); i < std::max(pixle_x[0], pixle_x[1]); i += resolution_)
        {
            geometry_msgs::msg::Point32 point;

            if (fabs(v_wall_.back().polygon.points[0].x - v_wall_.back().polygon.points[1].x) > fabs(v_wall_.back().polygon.points[0].y - v_wall_.back().polygon.points[1].y))
            {
                point.x = i;
                point.y = k * i + b;
            }
            else
            {
                point.x = k * i + b;
                point.y = i;
            }

            wallPoint_.back().polygon.points.push_back(point);
        }

        return true;
    }

    void VirtualWall::AddWallCallback(const geometry_msgs::msg::Polygon::SharedPtr msg)
    {

        RCLCPP_INFO(rclcpp::get_logger("VirtualWall"), "AddWallCallback()");
        // Xử lý dữ liệu kiểu geometry_msgs::msg::Polygon ở đây
        // Ví dụ:
        for (size_t i = 0; i < msg->points.size(); ++i)
        {
            geometry_msgs::msg::Point32 point = msg->points[i];
            // Thực hiện xử lý với từng điểm trong Polygon
            // Ví dụ: Lấy tọa độ x, y của mỗi điểm
            double x = point.x;
            double y = point.y;
            double z = point.z;

            // Thêm các xử lý khác tùy theo nhu cầu của ứng dụng

            wallmax_x_ = std::max(wallmax_x_, x);
            wallmax_y_ = std::max(wallmax_y_, y);
            wallmin_x_ = std::min(wallmin_x_, x);
            wallmin_y_ = std::min(wallmin_y_, y);

            if (v_wall_.empty())
            {
                tutorial_interfaces::msg::Wall wall;
                wall.id = 0;
                wall.polygon.points.push_back(point);
                v_wall_.push_back(wall);
            }

            else
            {
                if (v_wall_.back().polygon.points.size() == 1)
                {
                    if (v_wall_.size() == 1)
                    {
                        v_wall_.back().id = 0;
                    }
                    else
                    {
                        v_wall_.back().id = v_wall_[v_wall_.size() - 2].id + 1;
                    }
                    v_wall_.back().polygon.points.push_back(point);
                    wallPoint_.push_back(v_wall_.back());
                    // Thực hiện các thao tác khác như tạo ra các điểm ảo nếu cần
                    WallInterpolation();
                    // Cập nhật thông tin về các vách
                    // updateWall();
                }
                else if (v_wall_.back().polygon.points.size() == 2)
                {
                    tutorial_interfaces::msg::Wall wall;
                    wall.id = v_wall_.size();
                    wall.polygon.points.push_back(point);
                    v_wall_.push_back(wall);
                }
            }
        }
    }
    void VirtualWall::DeleteWallCallback(const std_msgs::msg::Int32::SharedPtr msg)
    {
        for (size_t i = 0; i < v_wall_.size(); i++)
        {
            if (v_wall_[i].id == msg->data)
            {
                v_wall_.erase(v_wall_.begin() + i);
                wallPoint_.erase(wallPoint_.begin() + i);
            }
        }
    }
}
3

Hi, several things here:

  1. please do not put all the configurations of navigation into a single config file. This only makes EVERYTHING A LOT MORE COMPLEX TO DEBUG. Separate the configs for the different parts of navigation (localization, path planning, etc…). Then, only share what is relevant to the problem.

  2. I can see a first error and is that your class VirtualWall is not inheriting from public nav2_costmap_2d::Layer

As it is explained in the course, Unit 3:

  • Your custom class name is GradientLayer and inherits from the base class named nav2_costmap_2d::Layer. It is from here that you inherit methods and override them.

Maybe you defined like that in the .hpp file? Please show.

  1. The Plugin declaration has to be at the end of the cpp file as explained here:
// This is the macro allowing a custom_nav2_costmap_plugin::GradientLayer class
// to be registered in order to be dynamically loadable of base type nav2_costmap_2d::Layer.
// Usually places in the end of cpp-file where the loadable class written.
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(custom_nav2_costmap_plugin::GradientLayer, nav2_costmap_2d::Layer)
  1. Do not use cout to print messages. Use RCLCPP_DEBUG instead so the messages are centralized by the ROS system, and we can be sure that we receive them (some cout messaged maybe cached and not shown on screen when they where generated). Example:
RCLCPP_DEBUG(rclcpp::get_logger(
      "nav2_costmap_2d"), "GradientLayer::onFootprintChanged(): num footprint points: %lu",
    layered_costmap_->getFootprint().size());
  1. What is the output you are getting when you launch everything? If your code is correct, you should at least see the message of the constructor of the class. If not, then it means that it is not being loaded and the error lies in the way the class is constructed or the way the plugin is specified. Can you please change all the cout by RCLCPP_DEBUG calls, add some of those on each function of your class, and then see the result?
4

This topic was automatically closed 5 days after the last reply. New replies are no longer allowed.