我正在嘗試在 Ros2 中使用 Drake。問題是，在我的類建構式中定義了我的機器人工廠后，訂閱者回呼中發生了一些奇怪的行為，我無法在節點死亡的情況下呼叫任何工廠函式。這就是我所擁有的
class Foo : public rclcpp::Node{
public:
Foo() : Node("foo_node") {
auto [plant_, scene_graph_] = drake::multibody::AddMultibodyPlantSceneGraph(&builder, 0.0);
plant = &plant_;
scene_graph = &scene_graph_;
auto parser = drake::multibody::Parser(plant, scene_graph);
parser.package_map().AddPackageXml(pkg_xml_dir);
auto model_ = parser.AddModelFromFile(model_path);
model = &model_;
plant->WeldFrames(plant->world_frame(), plant->GetFrameByName("base"));
plant->Finalize();
std::unique_ptr<drake::systems::Diagram<double>> diagram = builder.Build();
context = diagram->CreateDefaultContext();
plant_context = &plant->GetMyMutableContextFromRoot(context.get());
// first print
std::cout << "--------------" << std::endl;
std::cout << plant->is_finalized() << std::endl;
std::cout << plant->HasFrameNamed("link_3") << std::endl; // prints 1
std::cout << plant->HasFrameNamed("link_3", *model) << std::endl; // prints 1
std::cout << plant->HasFrameNamed("base", *model) << std::endl; // prints 1
std::cout << plant->num_frames() << std::endl; // prints N
std::cout << "--------------" << std::endl;
// Subscriber to update current state
state_current_sub = this->create_subscription<sensor_msgs::msg::JointState>("/joint_states", 10, std::bind(&Foo::update_current_state, this, _1));
private:
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr state_current_sub;
drake::systems::DiagramBuilder<double> builder;
drake::multibody::MultibodyPlant<double>* plant;
drake::geometry::SceneGraph<double>* scene_graph;
std::string pkg_xml_dir = "/path/to/package.xml";
std::string model_path = "/path/to/robot.urdf";
drake::systems::Context<double>* plant_context;
std::unique_ptr<drake::systems::Context<double>> context;
drake::TypeSafeIndex<drake::multibody::ModelInstanceTag>* model;
void update_current_state(const sensor_msgs::msg::JointState::SharedPtr msg){
// Second print
std::cout << "--------------" << std::endl;
std::cout << plant->is_finalized() << std::endl; // prints 1
std::cout << plant->HasFrameNamed("link_3") << std::endl; // prints 0
std::cout << plant->HasFrameNamed("link_3", *model) << std::endl; // Dies with 'there is no model instance id 0/2/(random number) in the model.'
std::cout << plant->HasFrameNamed("base", *model) << std::endl; //
std::cout << plant->num_frames() << std::endl; // prints N
std::cout << "--------------" << std::endl;
auto & ee_frame = plant->GetFrameByName("link_3"); // Can't find frame with that name
auto G = plant->CalcGravityGeneralizedForces(*plant_context); // Dies with exit code -11
}
}
在第二張印刷品中，一切都出錯了。呼叫與以前相同的函式（例如檢查幀是否存在）將不起作用，即使它是第一次。我的節點要么列印錯誤（框架不存在），要么死于exit code -11或there is no model instance id 0/2/(random number) in the model.，具體取決于我使用的功能。
我的猜測是植物的定義和使用存在一些問題。關于我應該改變什么的任何想法？
uj5u.com熱心網友回復：
首先，您是否能夠簡要閱讀drake-ros？
https://github.com/RobotLocomotion/drake-ros/tree/develop
它仍在開發中，但可能有有用的花絮/實用程式。德雷克網站底部提到了它：https ://drake.mit.edu/
其次，我相信你的工廠超出了范圍。
請注意，diagram實際上是呼叫了plant, scene_graph一次的新所有者。builder.Build()您必須將其存盤為成員變數。
如果你這樣做，那么plant應該留在范圍內，它應該可以解決這個問題。
有關更多資訊，請參閱：https
://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_diagram_builder.html
具體而言：
它是單次使用的：在呼叫 Build 或 BuildInto 之后，DiagramBuilder 放棄了組成系統的所有權，因此應該被丟棄
轉載請註明出處，本文鏈接：https://www.uj5u.com/shujuku/513862.html