I am trying to figure out how to write a hardware interface for my differential-drive-based mobile robot, so that I can use the ros_controls package and control the output velocities to the wheels (for smoother navigation).

I am running ROS Kinetic on Ubuntu 16.04 LTS.

I am aware of the diffdrivecontroller but by the time I had found it I already implemented the communication, the odometry feedback loop and splitting the cmd_vel into differential velocities (left wheel and right wheel velocities).

Block Diagram of Set-Up [Sorry about putting it on a link, but I don't have enough karma]

This is the status of my current set-up. I can spawn, list and load the controllers through controller_manager. However, my control loop is incomplete.

I have a few questions regarding ros_controllers:

1. Providing a set-point to the effort joint interface

I have two joints 'leftwheel' and 'rightwheel' that need to be controlled. They are exposed through an Effort Joint Interface. I want to control them with velocity. The /diff_vel topic has the left and right wheel set point velocities.

 JointHandle jointVelHandleRight(joint_state_interface_.getHandle("right_wheel"), &joint_velocity_command_);

As I understand, the jointvelocitycommand is the controlled output variable. How do I provide the set point?

2. Using the update() method

The body of the control loop currently looks like

robot.read();
controller_manager.update(ros::Time::now(), elapsed_time);
robot.write();

Can the update method be used to complete the feedback loop by giving a set point. How are the joint states updated?

I couldn't find the update() method in the controller manager documentation. How should I go about writing my own update function?

I have been through these tutorials:

Creating a hardware interface

Writing a new controller

Super helpful blogpost on implementing ros_control

and these repos:

Differential Drive ROS Robot Repo on Github

ROS Control Boilerplate

Husky Base

but I couldn't find what I was looking for.

Due to contractual reasons, I am unable to share code snippets at this point in time but I hope to get a clearer understanding of ros_control implementation.

Thank you for your help!