single_arm_controller.hpp

Go to the documentation of this file.

#ifndef WBC_ROS_SINGLE_ARM_CONTROLLER_HPP
#define WBC_ROS_SINGLE_ARM_CONTROLLER_HPP
 
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <robot_control_msgs/msg/joint_command.hpp>
#include <robot_control_msgs/msg/robot_state.hpp>
#include <robot_control_msgs/msg/rigid_body_state.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <wbc_msgs/msg/wbc_timing_stats.hpp>
#include <realtime_tools/realtime_buffer.hpp>
#include <realtime_tools/realtime_publisher.hpp>
 
#include <wbc/core/RobotModel.hpp>
#include <wbc/core/PluginLoader.hpp>
#include <wbc/core/Scene.hpp>
#include <wbc/core/QPSolver.hpp>
#include <wbc/tools/JointIntegrator.hpp>
#include <wbc/tasks/SpatialVelocityTask.hpp>
#include <wbc/tasks/JointVelocityTask.hpp>
#include <wbc/controllers/CartesianPosPDController.hpp>
#include <wbc/controllers/JointPosPDController.hpp>
#include <wbc/types/RigidBodyState.hpp>
#include <wbc/core/QuadraticProgram.hpp>
 
// auto-generated by generate_parameter_library
#include "single_arm_controller_parameters.hpp"
 
namespace wbc_ros{
    class SingleArmController : public rclcpp_lifecycle::LifecycleNode{
 
        using CommandMsg = robot_control_msgs::msg::JointCommand;
        using CommandPublisher = rclcpp::Publisher<CommandMsg>;
        using RTCommandPublisher = realtime_tools::RealtimePublisher<CommandMsg>;
 
        using RobotStateMsg = robot_control_msgs::msg::RobotState;
        using RobotStateMsgPtr = std::shared_ptr<RobotStateMsg>;
        using RobotStateSubscription = rclcpp::Subscription<RobotStateMsg>::SharedPtr;
        using RTRobotStateBuffer = realtime_tools::RealtimeBuffer<RobotStateMsgPtr>;
 
        using RigidBodyStateMsg = robot_control_msgs::msg::RigidBodyState;
        using RigidBodyStateMsgPtr = std::shared_ptr<RigidBodyStateMsg>;
        using RigidBodyStateSubscription = rclcpp::Subscription<RigidBodyStateMsg>::SharedPtr;
        using RTRigidBodyStateBuffer = realtime_tools::RealtimeBuffer<RigidBodyStateMsgPtr>;
 
        using JointCommandMsg = robot_control_msgs::msg::JointCommand;
        using JointCommandMsgPtr = std::shared_ptr<JointCommandMsg>;
        using JointCommandSubscription = rclcpp::Subscription<JointCommandMsg>::SharedPtr;
        using RTJointCommandBuffer = realtime_tools::RealtimeBuffer<JointCommandMsgPtr>;
 
        using JointWeightMsg = std_msgs::msg::Float64MultiArray;
        using JointWeightMsgPtr = std::shared_ptr<JointWeightMsg>;
        using JointWeightSubscription = rclcpp::Subscription<JointWeightMsg>::SharedPtr;
        using RTJointWeightBuffer = realtime_tools::RealtimeBuffer<JointWeightMsgPtr>;    
 
        using TimingStatsMsg = wbc_msgs::msg::WbcTimingStats;
        using TimingStatsPublisher = rclcpp::Publisher<TimingStatsMsg>;
        using RTTimingStatsPublisher = realtime_tools::RealtimePublisher<TimingStatsMsg>;
 
        public:
            SingleArmController(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
            ~SingleArmController(){}
 
            virtual CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
            virtual CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
            virtual CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
            virtual CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
            virtual CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override;
            virtual CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override;
    
            void updateController();
            void joint_weight_callback(const JointWeightMsgPtr msg);
            void robot_state_callback(const RobotStateMsgPtr msg);
            void ee_pose_callback(const RigidBodyStateMsgPtr msg);
            void joint_position_callback(const JointCommandMsgPtr msg);
            void elbow_pose_callback(const RigidBodyStateMsgPtr msg);
 
        protected:
            wbc::ScenePtr scene;
            wbc::RobotModelPtr robot_model;
            wbc::QPSolverPtr solver;
            wbc::types::JointState joint_state;
            bool has_robot_state;
            int update_rate;
            wbc::types::JointCommand solver_output;
            wbc::HierarchicalQP qp;
            wbc::JointIntegrator joint_integrator;
            wbc::CartesianPosPDControllerPtr ee_pose_controller;
            wbc::JointPosPDControllerPtr joint_pos_controller;
            wbc::CartesianPosPDControllerPtr elbow_pose_controller;
            wbc::SpatialVelocityTaskPtr ee_pose_task;
            wbc::JointVelocityTaskPtr joint_position_task;
            wbc::SpatialVelocityTaskPtr elbow_pose_task;
 
            CommandMsg solver_output_msg;
            CommandPublisher::SharedPtr solver_output_publisher;
            std::unique_ptr<RTCommandPublisher> rt_solver_output_publisher;
 
            TimingStatsMsg timing_stats;
            TimingStatsPublisher::SharedPtr timing_stats_publisher;
            std::unique_ptr<RTTimingStatsPublisher> rt_timing_stats_publisher;
 
            JointWeightSubscription joint_weight_subscriber;
            RTJointWeightBuffer rt_joint_weight_buffer;
            JointWeightMsgPtr joint_weight_msg;
 
            RobotStateSubscription robot_state_subscriber;
            RTRobotStateBuffer rt_robot_state_buffer;
            RobotStateMsgPtr robot_state_msg;
 
            RigidBodyStateSubscription ee_pose_subscriber;
            RTRigidBodyStateBuffer rt_ee_pose_buffer;
            RigidBodyStateMsgPtr ee_pose_msg;
            wbc::types::RigidBodyState ee_pose;
 
            JointCommandSubscription joint_position_subscriber;
            RTJointCommandBuffer rt_joint_position_buffer;
            JointCommandMsgPtr joint_position_msg;
            wbc::types::JointCommand joint_position;
 
            RigidBodyStateSubscription elbow_pose_subscriber;
            RTRigidBodyStateBuffer rt_elbow_pose_buffer;
            RigidBodyStateMsgPtr elbow_pose_msg;
            wbc::types::RigidBodyState elbow_pose;
 
            rclcpp::Time stamp;
            rclcpp::TimerBase::SharedPtr timer;
 
            std::vector<int> joint_idx_map;
 
            std::shared_ptr<single_arm_controller::ParamListener> param_listener;
            single_arm_controller::Params params;
    };
}
 
#endif