70 lines
3.9 KiB
C++
70 lines
3.9 KiB
C++
|
// 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("Navigate 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)&&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) {
|
||
|
|
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;}
|
||
|
|
}
|