behavior-tree/ros2/bt_executor/tools/native_navigation_probe.cpp

// Test-only live DDS probe. Build via tests/helpers/native_navigation_contract.py.
#include <bt_executor/ros_driver.hpp>
#include <filesystem>
#include <iostream>
#include <thread>

int main(int argc,char** argv) {
  if(argc!=4) return 2;
  const int scenario=std::stoi(argv[1]);
  const std::string journal=argv[2], ns=argv[3];
  if(ns.rfind("/sim/",0)!=0) return 3;
  rclcpp::init(0,nullptr);
  try {
    auto node=std::make_shared<rclcpp::Node>("native_navigation_probe",ns);
    bt_executor::RosDriver driver(*node,"robot_01",journal+"/uuids.jsonl",0.5,10000000000LL,robot_bt::Milliseconds(5000));
    robot_bt::TaskConfig task; task.route="LEGACY";
    driver.bind_task(task,robot_bt::SiteConfig{},1);
    robot_bt::ActiveGoalRegistry registry(driver,journal+"/goals.jsonl");
    auto until=robot_bt::SteadyClock::now()+std::chrono::seconds(12);
    while(!driver.ready(robot_bt::Skill::NAVIGATE)&&robot_bt::SteadyClock::now()<until) {
      rclcpp::spin_some(node); std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    if(!driver.ready(robot_bt::Skill::NAVIGATE)) throw std::runtime_error("NavigateToPose discovery timed out");
    robot_bt::GoalRequest request;
    request.goal_id="probe-"+std::to_string(scenario);request.robot_id="robot_01";
    request.trace.task_id=request.goal_id;request.trace.run_id=request.goal_id;request.trace.subtask_id="navigate";
    request.skill=robot_bt::Skill::NAVIGATE;request.registered_pose=robot_bt::Pose{"map",double(scenario),0,0,0,0,0,1};
    request.capture_after=node->now().nanoseconds();
    const auto started=registry.start(request,robot_bt::SteadyClock::now());
    if(!started) throw std::runtime_error("start refused");
    const std::string active_id=*started;
    bool canceled=false;
    until=robot_bt::SteadyClock::now()+std::chrono::seconds(12);
    while(robot_bt::SteadyClock::now()<until) {
      rclcpp::spin_some(node);registry.pump(robot_bt::SteadyClock::now());
      auto record=registry.find(active_id);
      if(!record) throw std::runtime_error("registry lost active goal: "+active_id);
      if((scenario==1||scenario==6||scenario==9)&&record->accepted&&!canceled) {
        registry.request_cancel(active_id,robot_bt::SteadyClock::now());canceled=true;
      }
      if(record->result) break;
      std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    // Pump beyond terminal delivery to expose accidental sends/retries in live transport.
    until=robot_bt::SteadyClock::now()+std::chrono::milliseconds(350);
    while(robot_bt::SteadyClock::now()<until) {
      rclcpp::spin_some(node);registry.pump(robot_bt::SteadyClock::now());
      std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    auto record=registry.find(active_id);
    if(!record||!record->result) throw std::runtime_error("result timeout");
    const auto& result=*record->result;
    bool blocked_redispatch=false;
    if(scenario==6||scenario==9||scenario==10) {
      auto next=request;next.goal_id+="-forbidden-retry";
      next.trace.subtask_id="navigate-forbidden-retry";++next.trace.attempt;
      blocked_redispatch=!registry.start(next,robot_bt::SteadyClock::now()).has_value();
    }
    nlohmann::json report={{"scenario",scenario},{"code",int(result.code)},{"stop",int(result.stop)},
      {"state",int(record->state)},{"robot_locked",registry.robot_locked("robot_01")},
      {"error_code",result.error_code},{"detail",result.detail},{"feedback_sequence",record->last_sequence},
      {"feedback",record->feedback_snapshot},{"wire_request_type",record->request.wire_request_type},
      {"wire_result_type",result.wire_result_type},{"wire_result_bytes",result.wire_result_snapshot.size()/2},
      {"response_valid",result.response.valid},{"mapping_count",driver.mappings().size()},
      {"unknown_stop_blocks_redispatch",blocked_redispatch}};
    std::cout<<report.dump()<<std::endl;
    rclcpp::shutdown();return 0;
  }catch(const std::exception& e){std::cerr<<e.what()<<std::endl;rclcpp::shutdown();return 1;}
}