Load Diff Drive Controller with C++ Code

Course Support ROS Control 101
1

Hello all,

I was able to load my standart joint controllers with C++ code. But we need to load diff drive controller also. But I can’t figure it out. I can’t find any examples. Could you suggest me any document or example.

#include <sstream>
#include <opl_str_control/opl_str_controller_manager.h>
#include <joint_limits_interface/joint_limits_interface.h>
#include <joint_limits_interface/joint_limits.h>
#include <joint_limits_interface/joint_limits_urdf.h>
#include <joint_limits_interface/joint_limits_rosparam.h>
#include <opl_str_hardware/robot_hardware.h>

using namespace hardware_interface;
using joint_limits_interface::JointLimits;
using joint_limits_interface::SoftJointLimits;
using joint_limits_interface::PositionJointSoftLimitsHandle;
using joint_limits_interface::PositionJointSoftLimitsInterface;

namespace opl_str_control
{
opl_str_controller_manager::opl_str_controller_manager(ros::NodeHandle& nh) : nh_(nh) {
    init();
    controller_manager_.reset(new controller_manager::ControllerManager(this, nh_));
    nh_.param("/opl_str/loop_hz", loop_hz_, 500.0);
    ros::Duration update_freq = ros::Duration(1.0/loop_hz_);
    non_realtime_loop_ = nh_.createTimer(update_freq, &opl_str_controller_manager::update, this, false, true);
}

opl_str_controller_manager::~opl_str_controller_manager() {

}

void opl_str_controller_manager::init() {
    // Get joint names
    joint_names_.push_back("base_to_lifter_joint");
    joint_names_.push_back("lifter_to_rotator_joint");
    //joint_names_.push_back("/opl_str/opl_str_base_controller");

    //nh_.getParam("/opl_str/hardware_interface/joints", joint_names_);
    num_joints_ = joint_names_.size();

    // Resize vectors
    joint_position_.resize(num_joints_);
    joint_velocity_.resize(num_joints_);
    joint_effort_.resize(num_joints_);
    joint_position_command_.resize(num_joints_);
    joint_velocity_command_.resize(num_joints_);
    joint_effort_command_.resize(num_joints_);

    // DIFF DRIVE
    // ???

    // LIFTER JOINT
    opl_str_hardware::Joint lifterJoint = ROBOT.getJoint("base_to_lifter_joint");

    // Create joint state interface
    JointStateHandle jointStateHandleForLifter(lifterJoint.name, &joint_position_[0], &joint_velocity_[0], &joint_effort_[0]);
    joint_state_interface_.registerHandle(jointStateHandleForLifter);

    JointHandle jointPositionHandleForLifter(jointStateHandleForLifter, &joint_position_command_[0]);
    //JointLimits limitsForLifter;
    //SoftJointLimits softLimitsForLifter;
    //getJointLimits(lifterJoint.name, nh_, limitsForLifter);
    //PositionJointSoftLimitsHandle jointLimitsHandleForLifter(jointPositionHandleForLifter, limitsForLifter, softLimitsForLifter);
    //positionJointSoftLimitsInterface.registerHandle(jointLimitsHandleForLifter);
    position_joint_interface_.registerHandle(jointPositionHandleForLifter);

    // Create effort joint interface
    JointHandle jointEffortHandleForLifter(jointStateHandleForLifter, &joint_effort_command_[0]);
    effort_joint_interface_.registerHandle(jointEffortHandleForLifter);

    // ROTATOR JOINT
    opl_str_hardware::Joint rotatorJoint = ROBOT.getJoint("lifter_to_rotator_joint");

    // Create joint state interface
    JointStateHandle jointStateHandleForRotator(rotatorJoint.name, &joint_position_[1], &joint_velocity_[1], &joint_effort_[1]);
    joint_state_interface_.registerHandle(jointStateHandleForRotator);
 
    JointHandle jointPositionHandleForRotator(jointStateHandleForRotator, &joint_position_command_[1]);
    //JointLimits limitsForLifter;
    //SoftJointLimits softLimitsForLifter;
    //getJointLimits(lifterJoint.name, nh_, limitsForLifter);
    //PositionJointSoftLimitsHandle jointLimitsHandleForLifter(jointPositionHandleForLifter, limitsForLifter, softLimitsForLifter);
    //positionJointSoftLimitsInterface.registerHandle(jointLimitsHandleForLifter);
    position_joint_interface_.registerHandle(jointPositionHandleForRotator);

    // Create effort joint interface
    JointHandle jointEffortHandleForRotator(jointStateHandleForRotator, &joint_effort_command_[1]);
    effort_joint_interface_.registerHandle(jointEffortHandleForRotator);

    registerInterface(&joint_state_interface_);
    registerInterface(&position_joint_interface_);
    registerInterface(&effort_joint_interface_);
    registerInterface(&positionJointSoftLimitsInterface);

}

void opl_str_controller_manager::update(const ros::TimerEvent& e) {
    elapsed_time_ = ros::Duration(e.current_real - e.last_real);
    read();
    controller_manager_->update(ros::Time::now(), elapsed_time_);
    write(elapsed_time_);        
}

void opl_str_controller_manager::read() {
    for (int i = 0; i < num_joints_; i++) {
        joint_position_[i] = ROBOT.getJoint(joint_names_[i]).read();  
    }
}

void opl_str_controller_manager::write(ros::Duration elapsed_time) {
    //positionJointSoftLimitsInterface.enforceLimits(elapsed_time);
    for (int i = 0; i < num_joints_; i++) {
        
        ROBOT.getJoint(joint_names_[i]).actuate(joint_position_command_[i], joint_velocity_command_[i]); 
    }
}
}
2

We solved the problem. But it has many details and seems that no one looking or writing here. If anyone need I can send later.

1 Like
3

Hello there,

First of all, let me apologize for not responding to you, it is totally our fault. Anyways, I’m happy to see that you managed to solve the issue by yourself. I understand that you’ve done this outside our courses, so would it be possible that you share with us a ROSject with the original issue and how you solved it?

Best,

4

Hello @albertoezquerro

I saw your message right now. I am a little bit busy and our code is used in company project. So I need to write a different package for demonstration. When I got more time I will add it to ROSject and inform you.

PS: As a brief, we simply load controllers seperately. And we wrote a custom Diff Drive package with C++. Actually we used Effort/PositionControllers at the beginning. But we realized that we need JointTrajectoryController/position_velocity_acceleration_controller and still don’t find a way to load this type of controller with C++