Custom controller: How to publish the state of joints

Course Support ROS Control 101
,
1

Hello,
I’m doing the create your control course and I noticed that there are some differences when using the example custom controller for joint 1 in the RRbot and using a effort_controllers/JointPositionController for the second joint. If you do a rostopic list, you can see this:
imagen

Where for the joint 2 you have the command and statetopics but for the first joint you only have one command topic.

the example code (slightly changed by me to create a PID controller) is this:

#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <pluginlib/class_list_macros.h>
#include <std_msgs/Float64.h>

/*
Here, you are just including some files from other packages that are needed in order to create the controller, 
and declaring a namespace for this class (namespaces provide a method to prevent name conflicts in large projects).
*/
namespace controller_ns {

/*
Here you are just declaring the class, which will be inherited from hardware_interface::EffortJointInterface. 
This means that this controller will be able to control only joints that use an effort interface.
*/
class MyPositionController : public controller_interface::Controller<hardware_interface::EffortJointInterface> {
public:
/*
Here, you are creating the init() function, which will be called when your controller is loaded by the controller manager. 
Inside this function, you will get the name of the joint that you will control from the Parameter Server first (so from the YAML file, which you will modify later), and then you will get the joint object to use in the realtime loop.
*/
  bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &n) {
    std::string my_joint;
    if (!n.getParam("joint", my_joint)) {
        ROS_ERROR("Could not find joint name");
        return false;
    }

    // Load gain using gains set on parameter server
    if (!n.getParam("kp", kp_))
    {
        ROS_ERROR("Could not find the gain parameter value");
        return false;
    }
     // Load gain using gains set on parameter server
    if (!n.getParam("ki", ki_))
    {
        ROS_ERROR("Could not find the gain parameter value");
        return false;
    }
     // Load gain using gains set on parameter server
    if (!n.getParam("kd", kd_))
    {
        ROS_ERROR("Could not find the gain parameter value");
        return false;
    }

    // Get joint handle from hardware interface
    joint_ = hw->getHandle(my_joint); // throws on failure
    command_position = joint_.getPosition();  // set the current joint goal to the current joint position

    // Start command subscriber
    sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &MyPositionController::setCommandCB, this);

    // Start realtime state publisher
    jointStatePublisher = node->advertise<sensor_msgs::JointState>("joint_state", 1);

    return true;
  }

/*
PID o EL CONTROL 
*/
  void update(const ros::Time &time, const ros::Duration &period) {

    
    double dt = 0.01;

    // compute position error
    double current_pos = joint_.getPosition();
    double error = command_position - current_pos;

    // P term
    double P = (error * kp_);

    // I term
    double integral = integral + (error * dt);
    double I = ki_ * integral;

    //D term
    double derivative = error / dt;
    double D = kd_ * derivative;

    double Out = P + I + D;
    
    joint_.setCommand(Out);

    //publish state
    

    jointStatePublisher.publish(jointState);

  }

  void setCommandCB(const std_msgs::Float64ConstPtr& msg)
  {
      command_position = msg->data;
  }
/*
Here, you are starting and stopping the controller.
Note that:
float and double are not of integral or enumeration type so they ahve to be declared as constexpr, or non-static.
*/
  void starting(const ros::Time &time) {}
  void stopping(const ros::Time &time) {}

private:
  hardware_interface::JointHandle joint_;
  double kp_;
  double ki_;
  double kd_;
  double command_position;
  ros::Subscriber sub_command_;
};

/*
Here, you are calling the special macro plugin PLUGINLIB_EXPORT_CLASS 
in order to allow this class to be dynamically loaded.
*/
PLUGINLIB_EXPORT_CLASS(controller_ns::MyPositionController,
                       controller_interface::ControllerBase);
} // namespace controller_ns

Please, can someone help me if I want to publish the the state of joint 1 just like joint2? It would be really great to plot both joint1 command and state to see the differences!

Thanks in advance

2

Hello @mikellasa,

first and foremost congrats for writing your own PID controller! You are pushing yourself to continue learning and expanding your knowledge!

Now to your question about the rrbot/joint1_position_controller/state topic:

If you do a rostopic info rrbot/joint2_position_controller/state topic you will see that this topic is published by Gazebo. Why is this? Because when you use ros_control with a simulated robot, Gazebo is acting as the hardware interface which causes the (simulated) robot to move. In the same way Gazebo publishes the joint states from what it reads in simulation to a topic.

In consequence it is not the controller who is publishing those topics it is the hardware interface, in this particular case the Gazebo Plugin that acts as or replaces a hardware interface.

I am not 100% sure but I guess is that you are not seeing the rrbot/joint1_position_controller/state topic being published by Gazebo because you are using a custom controller and to have all the same topics available maybe you need to create a custom Gazebo plugin.

The course does not cover how to create a custom Gazebo plugin for ros_control but I can point you to some additional info where you can start you research:

I hope I could help a little,

Roberto

3

Hello rzeger,

I managed to create my custom controller and publish the states too! the reason of not showing the state was that in my custom controller I created the publisher but I wasnt using the right messages to publish the states of joint1. I made a little change in the update loop:

//add timestamp to the message to plot later
    jointState.header.stamp = time;
    jointState.time_step = period.toSec();

    jointState.process_value = current_pos;

    //publish state
    jointStatePublisher.publish(jointState);

using control_msgs/jointcontrollerstate I was able to create a timestamp and the position message to publish and then plot it in the rqt.

Thanks for helping me!

1 Like
4

Hi @mikellasa,

I am so happy it worked finally! For the most part, this is really hard stuff, congratulations! If you ever decide to make your controller available I will surely try it.

Roberto

5

This topic was automatically closed 5 days after the last reply. New replies are no longer allowed.