实现行为树执行器、任务协调和技能接口
This commit is contained in:
@@ -0,0 +1,326 @@
|
||||
#include <bt_executor/ros_driver.hpp>
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <cerrno>
|
||||
#include <cmath>
|
||||
#include <iomanip>
|
||||
#include <fstream>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace bt_executor {
|
||||
using namespace robot_bt;
|
||||
RosTime ns(const builtin_interfaces::msg::Time& t) { return std::int64_t(t.sec) * 1000000000LL + t.nanosec; }
|
||||
builtin_interfaces::msg::Time stamp(RosTime n) {
|
||||
builtin_interfaces::msg::Time t; t.sec = static_cast<std::int32_t>(n / 1000000000LL);
|
||||
t.nanosec = static_cast<std::uint32_t>(n % 1000000000LL); return t;
|
||||
}
|
||||
iface::msg::TaskTrace trace_msg(const Trace& t) {
|
||||
iface::msg::TaskTrace m; m.task_id=t.task_id; m.subtask_id=t.subtask_id; m.run_id=t.run_id;
|
||||
m.attempt=t.attempt; m.task_revision=t.task_revision; m.plan_version=t.plan_version;
|
||||
m.execution_generation=t.execution_generation; return m;
|
||||
}
|
||||
Trace trace_core(const iface::msg::TaskTrace& m) {
|
||||
Trace t; t.task_id=m.task_id; t.subtask_id=m.subtask_id; t.run_id=m.run_id;
|
||||
t.attempt=m.attempt; t.task_revision=m.task_revision; t.plan_version=m.plan_version;
|
||||
t.execution_generation=m.execution_generation; return t;
|
||||
}
|
||||
NativeStatus native(rclcpp_action::ResultCode c) {
|
||||
switch(c) {case rclcpp_action::ResultCode::SUCCEEDED:return NativeStatus::SUCCEEDED;
|
||||
case rclcpp_action::ResultCode::ABORTED:return NativeStatus::ABORTED;
|
||||
case rclcpp_action::ResultCode::CANCELED:return NativeStatus::CANCELED;
|
||||
default:return NativeStatus::UNKNOWN;}
|
||||
}
|
||||
static Holding holding(std::uint8_t s) {
|
||||
switch(s) {case 0:return Holding::EMPTY; case 1:return Holding::HOLDING_TARGET;
|
||||
case 2:return Holding::HOLDING_OTHER; default:return Holding::UNKNOWN;}
|
||||
}
|
||||
static Pose pose_core(const geometry_msgs::msg::PoseStamped& m) {
|
||||
return {m.header.frame_id,m.pose.position.x,m.pose.position.y,m.pose.position.z,
|
||||
m.pose.orientation.x,m.pose.orientation.y,m.pose.orientation.z,m.pose.orientation.w};
|
||||
}
|
||||
static geometry_msgs::msg::PoseStamped pose_msg(const Pose& p, RosTime now) {
|
||||
geometry_msgs::msg::PoseStamped m; m.header.frame_id=p.frame_id; m.header.stamp=stamp(now);
|
||||
m.pose.position.x=p.x; m.pose.position.y=p.y; m.pose.position.z=p.z;
|
||||
m.pose.orientation.x=p.qx;m.pose.orientation.y=p.qy;m.pose.orientation.z=p.qz;m.pose.orientation.w=p.qw; return m;
|
||||
}
|
||||
static Pose point_core(const geometry_msgs::msg::PointStamped& p) {
|
||||
// Carrier for observed position only. This quaternion is never sent as a
|
||||
// manipulator command; legacy VLA goals carry semantic ObjectTarget/RegionTarget.
|
||||
return {p.header.frame_id,p.point.x,p.point.y,p.point.z,0,0,0,1};
|
||||
}
|
||||
static iface::msg::ObservationContext context_msg(const SnapshotMeta& v) {
|
||||
iface::msg::ObservationContext c;c.schema_version=v.schema_version;c.trace=trace_msg(v.trace);
|
||||
c.source_goal_id=v.source_goal_id;c.geometry_epoch=v.geometry_epoch;c.observed_at=stamp(v.observed_at);
|
||||
c.valid_until=stamp(v.valid_until);c.writer=v.writer;return c;
|
||||
}
|
||||
static SnapshotMeta context_core(const iface::msg::ObservationContext& c) {
|
||||
SnapshotMeta m;m.schema_version=c.schema_version;m.trace=trace_core(c.trace);
|
||||
m.source_goal_id=c.source_goal_id;m.geometry_epoch=c.geometry_epoch;m.observed_at=ns(c.observed_at);
|
||||
m.valid_until=ns(c.valid_until);m.writer=c.writer;return m;
|
||||
}
|
||||
static void durable_append(const std::string& path, const std::string& line) {
|
||||
const int fd=::open(path.c_str(),O_CREAT|O_WRONLY|O_APPEND,0600);
|
||||
if(fd<0)throw std::runtime_error("cannot open UUID journal");
|
||||
std::size_t offset=0;
|
||||
while(offset<line.size()) {auto n=::write(fd,line.data()+offset,line.size()-offset);
|
||||
if(n<0 && errno==EINTR)continue;
|
||||
if(n<=0){::close(fd);throw std::runtime_error("cannot write UUID journal");}offset+=n;}
|
||||
const int status=::fsync(fd);::close(fd);if(status)throw std::runtime_error("cannot sync UUID journal");
|
||||
}
|
||||
|
||||
RosDriver::RosDriver(rclcpp::Node& n, std::string robot_id, std::string journal,
|
||||
double confidence, std::int64_t lifetime, Milliseconds skill_timeout)
|
||||
:node_(n),robot_id_(std::move(robot_id)),uuid_journal_(std::move(journal)),
|
||||
min_confidence_(confidence),observation_lifetime_ns_(lifetime) {
|
||||
const auto timeout_ms=skill_timeout.count();
|
||||
if(timeout_ms<=0||timeout_ms>3600000)throw std::invalid_argument("skill timeout must be 1..3600000 ms");
|
||||
skill_timeout_.sec=static_cast<std::int32_t>(timeout_ms/1000);
|
||||
skill_timeout_.nanosec=static_cast<std::uint32_t>((timeout_ms%1000)*1000000);
|
||||
std::ifstream mappings(uuid_journal_);std::string line;
|
||||
while(std::getline(mappings,line))if(!line.empty()) {
|
||||
auto value=json::parse(line);const auto id=value.at("client_goal_id").get<std::string>();
|
||||
if(id.empty()||value.at("ros_goal_uuid").get<std::string>().size()!=32)throw std::runtime_error("corrupt UUID journal");
|
||||
mappings_[id]=std::move(value);
|
||||
}
|
||||
navigate_=rclcpp_action::create_client<Navigate>(&n,n.declare_parameter<std::string>("navigate_action","skills/navigate"));
|
||||
manipulate_=rclcpp_action::create_client<Manipulate>(&n,n.declare_parameter<std::string>("execute_manipulation_action","skills/execute_manipulation"));
|
||||
locate_=rclcpp_action::create_client<Locate>(&n,n.declare_parameter<std::string>("locate_shelf_column_action","skills/locate_shelf_column"));
|
||||
semantic_=rclcpp_action::create_client<Semantic>(&n,n.declare_parameter<std::string>("navigate_semantic_action","skills/navigate_semantic"));
|
||||
localize_=rclcpp_action::create_client<Localize>(&n,n.declare_parameter<std::string>("localize_target_3d_action","skills/localize_target_3d"));
|
||||
assess_=rclcpp_action::create_client<Assess>(&n,n.declare_parameter<std::string>("assess_grasp_action","skills/assess_grasp"));
|
||||
posture_=rclcpp_action::create_client<Posture>(&n,n.declare_parameter<std::string>("execute_posture_action","skills/execute_posture"));
|
||||
verify_=rclcpp_action::create_client<Verify>(&n,n.declare_parameter<std::string>("verify_state_action","skills/verify_state"));
|
||||
space_=rclcpp_action::create_client<Space>(&n,n.declare_parameter<std::string>("check_free_space_action","skills/check_free_space"));
|
||||
safety_sub_=n.create_subscription<iface::msg::SafetyState>("safety_state",rclcpp::QoS(1).reliable(),
|
||||
[this](iface::msg::SafetyState::ConstSharedPtr v){if(v->robot_id==robot_id_)safety_state_=*v;});
|
||||
robot_sub_=n.create_subscription<iface::msg::RobotState>("robot_state",rclcpp::QoS(1).reliable(),
|
||||
[this](iface::msg::RobotState::ConstSharedPtr v){if(v->robot_id==robot_id_)robot_state_=*v;});
|
||||
target_pub_=n.create_publisher<iface::msg::TargetBinding>("context/target_binding",rclcpp::QoS(1).reliable().transient_local());
|
||||
placement_pub_=n.create_publisher<iface::msg::PlacementBinding>("context/placement_binding",rclcpp::QoS(1).reliable().transient_local());
|
||||
}
|
||||
bool RosDriver::fresh(RosTime at,RosTime until,RosTime after)const {
|
||||
const auto now=node_.now().nanoseconds();
|
||||
return at>0 && at>=after && at<=now && until>now && until>=at &&
|
||||
now-at<=observation_lifetime_ns_;
|
||||
}
|
||||
SafetySnapshot RosDriver::safety(const std::string& target_id)const {
|
||||
SafetySnapshot s;
|
||||
if(!safety_state_||!robot_state_)return s;
|
||||
const auto& a=*safety_state_;const auto& b=*robot_state_;
|
||||
if(!fresh(ns(a.stamp),ns(a.valid_until))||!fresh(ns(b.stamp),ns(b.valid_until))||
|
||||
a.evidence_ref.empty()||b.evidence_ref.empty())return s;
|
||||
s.safe=a.safety_valid&&a.motion_allowed&&!a.emergency_stop_active&&!a.protective_stop_active&&!faulted_;
|
||||
s.stationary=b.base_stopped_valid&&b.base_stopped&&b.posture_settled_valid&&b.posture_settled;
|
||||
s.holding=holding(b.holding_state);
|
||||
if(s.holding==Holding::HOLDING_TARGET && b.held_target_ref!=target_id)s.holding=Holding::UNKNOWN;
|
||||
s.observed_at=std::min(ns(a.stamp),ns(b.stamp));s.valid_until=std::min(ns(a.valid_until),ns(b.valid_until));return s;
|
||||
}
|
||||
std::optional<std::uint64_t> RosDriver::geometry_epoch() const {
|
||||
if(!robot_state_||!fresh(ns(robot_state_->stamp),ns(robot_state_->valid_until))||robot_state_->evidence_ref.empty())return std::nullopt;
|
||||
return robot_state_->geometry_epoch;
|
||||
}
|
||||
bool RosDriver::ready(Skill skill)const {
|
||||
if(faulted_)return false;
|
||||
switch(skill) {
|
||||
case Skill::NAVIGATE:return current_task_.route=="LEGACY"?navigate_->action_server_is_ready():semantic_->action_server_is_ready();
|
||||
case Skill::PICK:case Skill::PLACE:return manipulate_->action_server_is_ready();
|
||||
case Skill::LOCATE_SHELF_COLUMN:return locate_->action_server_is_ready();
|
||||
case Skill::LOCALIZE_TARGET:return localize_->action_server_is_ready();
|
||||
case Skill::EVALUATE_GRASP:return assess_->action_server_is_ready();
|
||||
case Skill::ADJUST_POSTURE:case Skill::TRANSPORT_POSTURE:return posture_->action_server_is_ready();
|
||||
case Skill::VERIFY_EMPTY:case Skill::VERIFY_PICK:case Skill::VERIFY_TRANSPORT:case Skill::VERIFY_PLACE:return verify_->action_server_is_ready();
|
||||
case Skill::CHECK_FREE_SPACE:return space_->action_server_is_ready();
|
||||
}return false;
|
||||
}
|
||||
void RosDriver::record_mapping(const GoalRequest& r,const rclcpp_action::GoalUUID& uuid) {
|
||||
std::ostringstream hex;for(auto b:uuid)hex<<std::hex<<std::setfill('0')<<std::setw(2)<<unsigned(b);
|
||||
json m={{"client_goal_id",r.goal_id},{"ros_goal_uuid",hex.str()},{"task_id",r.trace.task_id},
|
||||
{"run_id",r.trace.run_id},{"task_revision",r.trace.task_revision},{"plan_version",r.trace.plan_version},
|
||||
{"execution_generation",r.trace.execution_generation},{"subtask_id",r.trace.subtask_id},{"attempt",r.trace.attempt}};
|
||||
durable_append(uuid_journal_,m.dump()+"\n");mappings_[r.goal_id]=m;
|
||||
}
|
||||
json RosDriver::mappings()const {json j=json::array();for(const auto& kv:mappings_)j.push_back(kv.second);return j;}
|
||||
void RosDriver::cancel(const std::string& id) {
|
||||
cancel_intents_.insert(id);auto it=cancelers_.find(id);if(it!=cancelers_.end())it->second();
|
||||
}
|
||||
std::vector<GoalEvent> RosDriver::drain_events(){std::vector<GoalEvent> v;v.swap(events_);return v;}
|
||||
ExecutionResult RosDriver::execution(const iface::msg::ExecutionResult& m,const GoalRequest& request)const {
|
||||
ExecutionResult r;r.detail=m.error_code+": "+m.message;
|
||||
switch(m.status){case 0:r.code=ResultCode::COMPLETED;break;case 1:r.code=ResultCode::FAILED;break;
|
||||
case 2:r.code=ResultCode::CANCELED;break;case 3:r.code=ResultCode::TIMED_OUT;break;
|
||||
case 4:r.code=ResultCode::REJECTED;break;default:return r;}
|
||||
// A stop label without a timestamp and evidence cannot release motion resources.
|
||||
if(m.stop_state==1&&!m.stop_evidence_ref.empty()&&fresh(ns(m.stopped_at),ns(m.stopped_at)+observation_lifetime_ns_,request.capture_after))
|
||||
r.stop=StopState::CONFIRMED;
|
||||
return r;
|
||||
}
|
||||
ExecutionResult RosDriver::readonly_result(rclcpp_action::ResultCode native_code,bool valid,SkillResponse response)const {
|
||||
ExecutionResult r;r.response=std::move(response);r.response.valid=valid;
|
||||
// These servers are contractually read-only. Their native terminal is enough
|
||||
// for this call's stopping; global fresh RobotState still gates every motion.
|
||||
r.stop=native_code==rclcpp_action::ResultCode::UNKNOWN?StopState::UNKNOWN:StopState::CONFIRMED;
|
||||
if(native_code==rclcpp_action::ResultCode::SUCCEEDED)r.code=ResultCode::COMPLETED;
|
||||
else if(native_code==rclcpp_action::ResultCode::CANCELED)r.code=ResultCode::CANCELED;
|
||||
else r.code=ResultCode::FAILED;
|
||||
return r;
|
||||
}
|
||||
SnapshotMeta RosDriver::meta(const GoalRequest& r,RosTime at,RosTime until,const std::string& writer)const {
|
||||
SnapshotMeta m;m.trace=r.trace;m.source_goal_id=r.goal_id;m.geometry_epoch=r.geometry_epoch;
|
||||
m.observed_at=at;m.valid_until=until;m.writer=writer;return m;
|
||||
}
|
||||
void RosDriver::send(const GoalRequest& r) {
|
||||
if(r.robot_id!=robot_id_)throw std::runtime_error("wrong robot namespace");
|
||||
const bool geometry_sensitive=r.skill==Skill::EVALUATE_GRASP||r.skill==Skill::PICK||r.skill==Skill::PLACE||
|
||||
r.skill==Skill::ADJUST_POSTURE||r.skill==Skill::TRANSPORT_POSTURE;
|
||||
if(geometry_sensitive) {
|
||||
const auto epoch=geometry_epoch();
|
||||
if(!epoch||*epoch!=r.geometry_epoch) {
|
||||
// No wire send occurred. This exact attempt is explicitly rejected and
|
||||
// still audited by the registry; no motion may use an old-epoch binding.
|
||||
GoalEvent rejected;rejected.kind=EventKind::REJECTED;rejected.goal_id=r.goal_id;rejected.trace=r.trace;
|
||||
events_.push_back(rejected);return;
|
||||
}
|
||||
}
|
||||
const auto now=node_.now().nanoseconds();
|
||||
switch(r.skill) {
|
||||
case Skill::NAVIGATE: {
|
||||
if(!r.registered_pose)throw std::runtime_error("registered navigation pose required");
|
||||
if(!r.navigation_kind.empty()) {
|
||||
Semantic::Goal g;g.trace=trace_msg(r.trace);g.kind=r.navigation_kind;g.reference=r.navigation_ref;g.shelf_id=r.shelf;g.side_id=r.side;g.column_id=r.column;g.tier_id=r.tier;g.registry_version=registry_version_;g.position_tolerance=r.position_tolerance_m;g.orientation_tolerance=r.orientation_tolerance_rad;g.timeout=timeout();
|
||||
send_typed<Semantic>(semantic_,g,r,6,[this,r](const Semantic::Result& m,auto){
|
||||
auto out=execution(m.result,r);out.response.valid=m.pose_valid&&m.errors_valid&&std::isfinite(m.final_position_error)&&std::isfinite(m.final_orientation_error)&&m.final_position_error>=0&&m.final_position_error<=r.position_tolerance_m&&std::abs(m.final_orientation_error)<=r.orientation_tolerance_rad;
|
||||
if(m.pose_valid)out.response.final_pose=pose_core(m.final_pose);out.response.base_stopped=out.stop==StopState::CONFIRMED;return out;
|
||||
});break;
|
||||
}
|
||||
Navigate::Goal g;g.trace=trace_msg(r.trace);g.target_pose=pose_msg(*r.registered_pose,now);
|
||||
g.position_tolerance=r.position_tolerance_m;g.orientation_tolerance=r.orientation_tolerance_rad;g.timeout=timeout();
|
||||
send_typed<Navigate>(navigate_,g,r,6,[this,r](const Navigate::Result& m,auto){
|
||||
auto out=execution(m.result,r);out.response.valid=m.pose_valid&&m.errors_valid&&
|
||||
std::isfinite(m.final_position_error)&&std::isfinite(m.final_orientation_error)&&
|
||||
m.final_position_error>=0&&std::abs(m.final_orientation_error)<=std::acos(-1.0)&&
|
||||
m.final_position_error<=r.position_tolerance_m&&std::abs(m.final_orientation_error)<=r.orientation_tolerance_rad;
|
||||
if(m.pose_valid)out.response.final_pose=pose_core(m.final_pose);
|
||||
out.response.base_stopped=out.stop==StopState::CONFIRMED;return out;});break;
|
||||
}
|
||||
case Skill::PICK:case Skill::PLACE: {
|
||||
Manipulate::Goal g;g.trace=trace_msg(r.trace);g.skill=r.skill==Skill::PICK?"pick":"place";
|
||||
g.instruction=g.skill+" registered target "+r.target_id;g.target.object_ref=r.target_id;g.target.description=r.target_id;
|
||||
if(r.skill==Skill::PLACE){g.destination.region_ref=r.destination_id;g.destination.description=r.destination_id;}
|
||||
g.timeout=timeout();
|
||||
send_typed<Manipulate>(manipulate_,g,r,5,[this,r](const Manipulate::Result& m,auto){
|
||||
auto out=execution(m.result,r);out.response.valid=!m.execution_record_ref.empty();
|
||||
out.response.base_stopped=out.stop==StopState::CONFIRMED;return out;});break;
|
||||
}
|
||||
case Skill::LOCATE_SHELF_COLUMN: {
|
||||
Locate::Goal g;g.task_id=r.trace.task_id;g.subtask_id=r.trace.subtask_id;g.target_ref=r.target_id;
|
||||
g.target_description=r.target_id;g.observation_station_id=current_task_.observe_location;g.station_registry_version=registry_version_;
|
||||
g.source_region_ref=r.shelf;g.capture_after=stamp(r.capture_after);g.timeout=timeout();
|
||||
send_typed<Locate>(locate_,g,r,1,[this,r](const Locate::Result& m,auto code){
|
||||
SkillResponse out;out.shelf=m.shelf_id;out.side=m.side_id;out.column=m.column_id;out.tier=m.tier_id;
|
||||
bool ok=m.status==0&&std::isfinite(m.confidence)&&m.confidence>=min_confidence_&&m.confidence<=1&&
|
||||
!m.observation_id.empty()&&!m.record_ref.empty()&&!m.shelf_id.empty()&&!m.side_id.empty()&&!m.column_id.empty()&&
|
||||
fresh(ns(m.observed_at),ns(m.observed_at)+observation_lifetime_ns_,r.capture_after);
|
||||
if(ok)shelf_bindings_[r.trace.run_id]={{"shelf",m.shelf_id},{"side",m.side_id},{"column",m.column_id},{"tier",m.tier_id},{"record",m.record_ref}};
|
||||
return readonly_result(code,ok,out);});break;
|
||||
}
|
||||
case Skill::LOCALIZE_TARGET: {
|
||||
Localize::Goal g;g.task_id=r.trace.task_id;g.subtask_id=r.trace.subtask_id;
|
||||
g.target_ref=r.target_id;g.shelf_id=r.shelf;g.capture_after=stamp(r.capture_after);
|
||||
g.target_description=r.target_id;
|
||||
g.expected_geometry_epoch=r.geometry_epoch;g.timeout=timeout();
|
||||
auto binding=shelf_bindings_.find(r.trace.run_id);
|
||||
if(binding!=shelf_bindings_.end()){g.column_id=binding->second.at("column");g.tier_id=binding->second.at("tier");g.station_binding_ref=binding->second.at("record");}
|
||||
send_typed<Localize>(localize_,g,r,1,[this,r](const Localize::Result& m,auto code){
|
||||
SkillResponse out;const auto at=ns(m.target_point.header.stamp);
|
||||
bool ok=m.status==0&&m.geometry_valid&&m.target_ref==r.target_id&&m.geometry_epoch==r.geometry_epoch&&
|
||||
!m.observation_id.empty()&&!m.calibration_id.empty()&&!m.record_ref.empty()&&!m.quality_code.empty()&&m.measurement_source<=2&&
|
||||
m.position_error_bound_valid&&std::isfinite(m.position_error_bound)&&m.position_error_bound>=0&&
|
||||
fresh(at,at+observation_lifetime_ns_,r.capture_after)&&
|
||||
fresh(ns(m.rgb_stamp),ns(m.rgb_stamp)+observation_lifetime_ns_,r.capture_after)&&
|
||||
fresh(ns(m.depth_stamp),ns(m.depth_stamp)+observation_lifetime_ns_,r.capture_after);
|
||||
TargetBinding b;b.meta=meta(r,at,at+observation_lifetime_ns_,"localize_target_3d");
|
||||
b.target_id=m.target_ref;b.pose=point_core(m.target_point);ok=ok&&valid_pose(b.pose);
|
||||
if(m.grasp_point_valid)ok=ok&&valid_pose(point_core(m.grasp_point));
|
||||
if(ok){out.target=b;iface::msg::TargetBinding msg;msg.context=context_msg(b.meta);msg.context.observation_id=m.observation_id;
|
||||
msg.target.object_ref=m.target_ref;msg.target.description=r.target_id;msg.target_point=m.target_point;
|
||||
msg.grasp_point=m.grasp_point;msg.grasp_point_valid=m.grasp_point_valid;msg.grasp_region_ref=m.grasp_region_ref;
|
||||
msg.geometry_valid=true;msg.calibration_id=m.calibration_id;msg.shelf_id=r.shelf;target_pub_->publish(msg);}
|
||||
return readonly_result(code,ok,out);});break;
|
||||
}
|
||||
case Skill::EVALUATE_GRASP: {
|
||||
if(!r.target||!robot_state_)throw std::runtime_error("assess missing binding/state");
|
||||
Assess::Goal g;g.trace=trace_msg(r.trace);g.target_binding.context=context_msg(r.target->meta);
|
||||
g.target_binding.target.object_ref=r.target_id;g.target_binding.target.description=r.target_id;
|
||||
g.target_binding.geometry_valid=true;
|
||||
g.target_binding.target_point.header.frame_id=r.target->pose.frame_id;
|
||||
g.target_binding.target_point.header.stamp=stamp(r.target->meta.observed_at);
|
||||
g.target_binding.target_point.point.x=r.target->pose.x;g.target_binding.target_point.point.y=r.target->pose.y;g.target_binding.target_point.point.z=r.target->pose.z;
|
||||
g.robot_state=*robot_state_;g.allowed_posture_ids=allowed_postures_;g.timeout=timeout();
|
||||
send_typed<Assess>(assess_,g,r,255,[this,r](const Assess::Result& m,auto code){
|
||||
SkillResponse out;out.posture_id=m.posture_id;
|
||||
switch(m.decision){case 0:out.admission=Admission::DIRECT;break;case 1:out.admission=Admission::ADJUST_POSTURE;break;
|
||||
case 2:out.admission=Admission::NOT_REACHABLE;break;default:out.admission=Admission::UNKNOWN;}
|
||||
bool ok=m.decision<=3&&m.geometry_epoch==r.geometry_epoch&&!m.evidence_ref.empty();
|
||||
return readonly_result(code,ok,out);});break;
|
||||
}
|
||||
case Skill::ADJUST_POSTURE:case Skill::TRANSPORT_POSTURE: {
|
||||
Posture::Goal g;g.trace=trace_msg(r.trace);g.posture_id=r.posture_id;
|
||||
g.expected_geometry_epoch=r.geometry_epoch;g.timeout=timeout();
|
||||
send_typed<Posture>(posture_,g,r,3,[this,r](const Posture::Result& m,auto){
|
||||
auto out=execution(m.result,r);const auto& state=m.robot_state;
|
||||
out.response.valid=state.robot_id==robot_id_&&state.posture_id==r.posture_id&&
|
||||
state.posture_settled_valid&&state.posture_settled&&state.base_stopped_valid&&state.base_stopped&&
|
||||
m.geometry_epoch==r.geometry_epoch+1&&state.geometry_epoch==m.geometry_epoch&&
|
||||
!state.evidence_ref.empty()&&fresh(ns(state.stamp),ns(state.valid_until),r.capture_after);
|
||||
out.response.posture_id=state.posture_id;out.response.base_stopped=state.base_stopped_valid&&state.base_stopped;
|
||||
out.response.holding=holding(state.holding_state);return out;});break;
|
||||
}
|
||||
case Skill::VERIFY_EMPTY:case Skill::VERIFY_PICK:case Skill::VERIFY_TRANSPORT:case Skill::VERIFY_PLACE: {
|
||||
Verify::Goal g;g.trace=trace_msg(r.trace);g.source_goal_id=r.goal_id;
|
||||
g.check=r.skill==Skill::VERIFY_EMPTY?0:r.skill==Skill::VERIFY_PICK?1:r.skill==Skill::VERIFY_TRANSPORT?2:3;
|
||||
g.target.object_ref=r.target_id;g.target.description=r.target_id;
|
||||
g.destination.region_ref=r.destination_id;g.destination.description=r.destination_id;
|
||||
g.expected_geometry_epoch=r.geometry_epoch;
|
||||
g.capture_after=stamp(r.capture_after);g.timeout=timeout();
|
||||
send_typed<Verify>(verify_,g,r,255,[this,r](const Verify::Result& m,auto code){
|
||||
const auto& e=m.evidence;SkillResponse out;out.evidence=context_core(e.context);
|
||||
out.target_id=e.target_ref;out.destination_id=e.destination_ref;out.holding=holding(e.holding_state);
|
||||
out.base_stopped=e.stopped_valid&&e.stopped;out.in_destination=e.target_in_destination_valid&&e.target_in_destination;
|
||||
bool ok=e.context.schema_version==1&&same_trace(trace_core(e.context.trace),r.trace)&&
|
||||
e.context.source_goal_id==r.goal_id&&e.context.geometry_epoch==r.geometry_epoch&&
|
||||
!e.context.writer.empty()&&!e.context.observation_id.empty()&&!e.evidence_ref.empty()&&!e.source.empty()&&
|
||||
fresh(ns(e.context.observed_at),ns(e.context.valid_until),r.capture_after)&&e.target_ref==r.target_id;
|
||||
out.verified=e.status==0&&out.base_stopped;
|
||||
if(r.skill!=Skill::VERIFY_EMPTY)out.verified=out.verified&&e.target_match_valid&&e.target_match;
|
||||
if(r.skill==Skill::VERIFY_PICK||r.skill==Skill::VERIFY_TRANSPORT)
|
||||
out.verified=out.verified&&e.grasp_stable_valid&&e.grasp_stable&&out.holding==Holding::HOLDING_TARGET;
|
||||
else if(r.skill==Skill::VERIFY_EMPTY)out.verified=out.verified&&e.hand_empty_valid&&e.hand_empty&&out.holding==Holding::EMPTY;
|
||||
else out.verified=out.verified&&e.destination_ref==r.destination_id&&e.hand_empty_valid&&e.hand_empty&&
|
||||
out.holding==Holding::EMPTY&&out.in_destination;
|
||||
return readonly_result(code,ok&&e.status<=2,out);});break;
|
||||
}
|
||||
case Skill::CHECK_FREE_SPACE: {
|
||||
Space::Goal g;g.task_id=r.trace.task_id;g.subtask_id=r.trace.subtask_id;g.destination_ref=r.destination_id;
|
||||
g.object_ref=r.target_id;g.object_description=r.target_id;g.destination_description=r.destination_id;
|
||||
g.capture_after=stamp(r.capture_after);g.placement_constraints_json="{}";g.timeout=timeout();
|
||||
send_typed<Space>(space_,g,r,1,[this,r](const Space::Result& m,auto code){
|
||||
SkillResponse out;bool ok=m.status==0&&m.destination_ref==r.destination_id&&m.geometry_valid&&
|
||||
std::isfinite(m.confidence)&&m.confidence>=min_confidence_&&m.confidence<=1&&
|
||||
!m.observation_id.empty()&&!m.record_ref.empty()&&!m.placement_region_ref.empty()&&!m.quality_code.empty()&&
|
||||
fresh(ns(m.observed_at),ns(m.valid_until),r.capture_after)&&(m.placement_pose_valid||m.placement_point_valid);
|
||||
PlacementBinding b;b.meta=meta(r,ns(m.observed_at),ns(m.valid_until),"check_free_space");
|
||||
b.target_id=r.target_id;b.destination_id=m.destination_ref;b.free_space_confirmed=ok;
|
||||
if(m.placement_pose_valid)b.pose=pose_core(m.placement_pose);else if(m.placement_point_valid)b.pose=point_core(m.placement_point);
|
||||
ok=ok&&valid_pose(b.pose);if(ok){out.placement=b;iface::msg::PlacementBinding msg;
|
||||
msg.context=context_msg(b.meta);msg.context.observation_id=m.observation_id;
|
||||
msg.destination.region_ref=m.destination_ref;msg.destination.description=r.destination_id;
|
||||
msg.placement_region_ref=m.placement_region_ref;msg.placement_point=m.placement_point;
|
||||
msg.placement_point_valid=m.placement_point_valid;msg.placement_pose=m.placement_pose;
|
||||
msg.placement_pose_valid=m.placement_pose_valid;msg.geometry_valid=true;placement_pub_->publish(msg);}
|
||||
return readonly_result(code,ok,out);});break;
|
||||
}
|
||||
}
|
||||
}
|
||||
} // namespace bt_executor
|
||||
Reference in New Issue
Block a user