Questions with NDT Localization and Vehicle Status

[shpark98]

Hello,

I am trying to use Autoware with ROS2 for the Agile Scout robot. While everything works well for research purposes in ROS1, I am facing issues in ROS2 and would like to ask for help.

I have two questions regarding the issues:

When performing NDT scan matching, the robot continues to drive even when the score is low. In the terminal, a warning is displayed indicating that the NDT score is below the threshold. Shouldn't the robot stop driving and perform a process to re-localize itself when the score is low? If this is the correct behavior, could you let me know which settings I need to adjust?

Screencast from 01-06-2025 08:06:01 PM.webm
In the video, the robot is not physically moving, so the LiDAR is receiving data from the same location. However, when I click "auto" in RViz, the robot continues to move in simulation, regardless of the NDT localization status.

Is it correct that the vehicle's status needs to be received for NDT scan matching? If so, is it sufficient to publish the velocity status of the vehicle by taking the velocity information from the Scout's odometry and publishing it to the velocity_status topic?

After writing the code and checking with rqt_graph, I noticed that the /control/vehicle/status/velocity_status topic is being published from two sources: one from the Scout's odometry and another from the Simple Simulator. This seems problematic. How should I address this issue?

[TaikiYamada4]

Hi, @shpark98

1. When the NDT score is low, ndt_scan_matcher will not publish the estimated pose, BUT the latter ekf_localizer will keep outputting the vehicle pose by dead reckoning using the vehicle velocity. That's why the robot is moving.
2. In terms of velocity topic input, ndt_scan_matcher doesn't need the velocity but the latter ekf_localizer needs it.

You mentioned that the robot is not moving but the upper part of the rviz shows us non-zero velocity and steering, and I think that's the main problem. Is there a module publishing fake velocity?

[TaikiYamada4]

I’m considering creating a bridge system. Do you have any references I could look into?

These pages might be helpful. The "bridge" we are talking about is named as "Vehicle Interface" in this document.

https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/creating-vehicle-interface-package/vehicle-interface/
https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/creating-vehicle-interface-package/creating-vehicle-interface/

[shpark98]

If the "bridge" refers to the vehicle interface, I was already using the vehicle interface before.

In the vehicle interface, as shown in the image, it provides information such as battery_charge, control_mode, gear_status, hazard_lights_status, steering_status, turn_indicators_status, and velocity_status to Autoware. Among these, are there any information that are mandatory to provide?

Previously, I was only providing vehicle_status.

I create a cmd_vel topic from control/command/control_cmd received from Autoware and send it to the robot.

Then, I receive the robot's /odom topic, and generate velocity_status, and send it back to Autoware.

this is my sample_vehicle_interface.cpp code

#include "sample_vehicle_interface.hpp"

VehicleInterface::VehicleInterface() : Node("vehicle_interface")
{
    control_cmd_sub_ = this->create_subscription<autoware_control_msgs::msg::Control>(
        "/control/command/control_cmd",
        10,
        std::bind(&VehicleInterface::callbackControlCmd, this, std::placeholders::_1));

    odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
        "/odom",
        10,
        std::bind(&VehicleInterface::odomCallback, this, std::placeholders::_1));

    twist_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
    gear_status_pub_ = this->create_publisher<autoware_vehicle_msgs::msg::GearReport>(
        "/vehicle/status/gear_status", 10);
    velocity_pub_ = this->create_publisher<autoware_vehicle_msgs::msg::VelocityReport>(
        "/vehicle/status/velocity_status", 10);
}

void VehicleInterface::callbackControlCmd(const autoware_control_msgs::msg::Control::SharedPtr msg)
{
    to_vehicle(msg);
    from_vehicle();
}

void VehicleInterface::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
{
    last_odom_msg_ = msg;
}

void VehicleInterface::to_vehicle(const autoware_control_msgs::msg::Control::SharedPtr msg)
{
    double linear_velocity = msg->longitudinal.velocity;
    double steering_angle = msg->lateral.steering_tire_angle;

    geometry_msgs::msg::Twist twist_msg;
    twist_msg.linear.x = linear_velocity;
    twist_msg.angular.z = steering_angle;

    twist_publisher_->publish(twist_msg);
}

void VehicleInterface::from_vehicle()
{
    if (!last_odom_msg_)
    {
        RCLCPP_WARN(this->get_logger(), "No odom message received yet");
        return;
    }

    float odom_longitudinal_velocity = last_odom_msg_->twist.twist.linear.x;
    float odom_lateral_velocity = last_odom_msg_->twist.twist.linear.y;
    float odom_heading_rate = last_odom_msg_->twist.twist.angular.z;


    autoware_vehicle_msgs::msg::VelocityReport velocity_msg;
    velocity_msg.header=last_odom_msg_->header;
    velocity_msg.header.frame_id = "base_link";
    velocity_msg.longitudinal_velocity = odom_longitudinal_velocity;
    velocity_msg.lateral_velocity = odom_lateral_velocity;
    velocity_msg.heading_rate = odom_heading_rate;
    velocity_pub_->publish(velocity_msg);
}

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<VehicleInterface>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

[TaikiYamada4]

@shpark98

I'm not sure this page is accurate, but I think velocity_status and control_mode is mandatory. (Maybe control_mode is also ignore-able)
https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/vehicle/

[shpark98]

To test whether the map might be causing the issue, I tried running it using planning_simulator.xml. In this case, I observed that the topics related to MissionPlanning (like route_marker) and those related to ScenarioPlanning (like Trajectory or LaneDriving) were all displayed correctly in RViz.

However, when running Autoware.launch, I noticed that only the route_marker topic related to MissionPlanning was present.

It seems that even with just the route_marker, driving should be possible. However, when I connect the vehicle interface(only with velocity_status) and press the "Auto" button, neither the robot nor the robot model in RViz responds at all.

[TaikiYamada4]

@shpark98

I'm sorry, I'm not that familiar with the planning/control component and it is hard to tell what is going wrong here.

Besides, we came quite far from the discussion title, and I recommend you to close here and create a new discussion so that someone familiar to this problem can catch. Attaching the log from Autoware to it might be helpful.