I am trying to use Drake in Ros2. The problem is that after defining my robot plant in my class constructor, some weird behavior occurs in the subscriber callback and I can't call any plant functions without the node dying. Here's what I have

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
        }

}

In the second print everything goes wrong. Calling the same functions as before, for example to check if a frame exists, won't work, even though it does the first time. My node either prints something wrong (that the frame doesn't exists), or dies with exit code -11 or with there is no model instance id 0/2/(random number) in the model., depending on which functions I use.

My guess is that there is some problem with the plant definition and usage. Any ideas on what should I change ?

CodePudding user response：

First off, have you been able to briefly peruse drake-ros?
https://github.com/RobotLocomotion/drake-ros/tree/develop
It's still under development, but may have useful tidbits / utilities. It is mentioned at the bottom of Drake's website: https://drake.mit.edu/

Second off, I believe your plant is going out of scope.

Please note that diagram is actually the new owner of plant, scene_graph once builder.Build() is called. You must store that as a member variable.

If you do that, then plant should stay in scope, and it should resolve this issue.

For more info, see: https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_diagram_builder.html
Specifically:

It is single use: after calling Build or BuildInto, DiagramBuilder gives up ownership of the constituent systems, and should therefore be discarded