Main Application

The specific use case application, also known as the decisional program, must adhere to certain rules to integrate with the RVC Framework.

Main Application Development

This RVC Component is centered around the motion controller aspect of the RVC Framework. The workflow, in a high-level detail, is as follows:

• An API message, output rvc_messages, is received.

• The active grasp plugin computes the pre-grasp and grasp pose for the gripper, also known as the Tool Center Point (TCP).

• A State machine or an equivalent programmatic approach is used to decide actions and, if applicable, move the robot to the pre-grasp and then grasp positions, activating the gripper via the selected RVCMotionController::RVCMotionControllerInterface based plugin.

State Machine Snippet

The ROS2 node that implements the decisional process needs to load two plugins according to ROS2 parameters. These parameters can be loaded via a launcher, command line, or yaml file.

Example in C++

#include "rvc_dynamic_motion_controller_use_case/state_machine.hpp"

#include "rvc_motion_controller_interface/rvc_motion_controller_interface.hpp"
#include "rvc_grasp_interface/rvc_grasp_interface.hpp"
#include <pluginlib/class_loader.hpp>

int main(int argc, char ** argv)
{
    rclcpp::init(argc, argv);

    auto stateMachine = std::make_shared<StateMachine>();

    stateMachine->declare_parameter<std::string>("motion_controller",
        "RVCMotionController::Moveit2ServoMotionController");
    stateMachine->declare_parameter<std::string>("grasp_plugin", "RVCControl::NonOrientedGrasp");
    std::string motionControllerName = stateMachine->get_parameter("motion_controller").as_string();
    std::string graspPluginName = stateMachine->get_parameter("grasp_plugin").as_string();

    pluginlib::ClassLoader<RVCMotionController::RVCMotionControllerInterface> motionControllerLoader(
        "rvc_motion_controller_interface", "RVCMotionController::RVCMotionControllerInterface");
    std::shared_ptr<RVCMotionController::RVCMotionControllerInterface> motionController;
    pluginlib::ClassLoader<RVCControl::RVCGraspInterface> graspLoader("rvc_grasp_interface",
        "RVCControl::RVCGraspInterface");
    std::shared_ptr<RVCControl::RVCGraspInterface> graspPlugin;

    motionController = motionControllerLoader.createSharedInstance(motionControllerName);

    if (!motionController->init(stateMachine))
    {
        return 1;
    }

    graspPlugin = graspLoader.createSharedInstance(graspPluginName);

    if (!graspPlugin->init(stateMachine))
    {
        return 1;
    }

    bool ret = stateMachine->init(motionController.get(), graspPlugin.get());

    if (ret)
    {
        rclcpp::spin(stateMachine);
    }

    rclcpp::shutdown();

    return 0;
}

This application will create an instance of a “StateMachine” class, inheriting from rclcpp::Node from ROS2 framework [TODO: link], and passing the smart pointers of the grasp and motion controller Interface instances.

For Example

#include "rclcpp/rclcpp.hpp"

class StateMachine : public rclcpp::Node
{
private:

    /// @brief internal run function to be called on a fixed timer to handle the state machine
    void run(void);
public:
    /// @brief Constructor, parameterless
    StateMachine();
    bool init(RVCMotionControllerInterface *motionController, RVCControl::RVCGraspInterface *graspPlugin);
};

where init will store the interface references and start the state machine internal processing.

Two more practical example are redistributed with rvc_dynamic_motion_controller_use_case and rvc_static_motion_controller_use_case state_machine.cpp and .hpp