Custom hardware interface gives command spike on target change

edit

I am trying to create a custom hardware interface, and have been following this guide to make one for a robot which uses effort_controllers/JointPositionController. To check that the HWI I made works, I made some modifications so that it publishes the calculated joint_effort_commands to a topic /cmd_effort, and subscribes to /rrbot/joint_states from the Gazebo ros control tutorial. I launch Gazebo and the controller as the tutorial describes, then I launch my own hardware interface and begin sending commands such as rostopic pub -1 /rrbot/joint1_position_controller/command std_msgs/Float64 "data: 1.5". I should probably mention that I was a little lazy when naming my HWI, so I think the controller manager that the Gazebo tutorial launches is replaced because I receive the following warning after launching my HWI:

Shutdown request received.

Reason given for shutdown: [new node registered with same name]

When comparing my published command /cmd_effort to the commands from Gazebo, such as /rrbot/joint1_position_controller/state/command, I see that /cmd_effort has a large spike when the target position is changed, but Gazebo does not. It's not obvious in the plot below, but the spike occurs for a single time step.

After some digging around, I determined that the spike was caused by the PID controller calculating error_dot manually, instead of using the actual joint velocity. Meaning that when I updated the target position, error_dot would spike momentarily because of the instantaneous increase in error. I calculated the command returned from the PID manually to confirm this (the spikes in my calculation are caused by Gazebo publishing a zero joint velocity for some reason).

My question then is how do I tell my HWI to provide the joint velocity to the joint position controller? This is assuming I'm able to acquire the joint velocity. Line 208 in joint_position_controller.cpp seems to be what I want, but I have not been able to find anything on how to set command_struct_.has_velocity_ so that line is executed.

I am running ROS kinetic with Gazebo 7 on Ubuntu 16.04. I've included my HWI below, but please let me know if additional source files would be useful.

#include <sstream>
#include <rrbot_unity_hwi/rrbot_hwi.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 <ROBOTcpp/ROBOT.h> // This robot object would be defined for a normal robot
#include <string>
#include <cstring>

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 rrbot_hardware_interface
{
    rrbotHardwareInterface::rrbotHardwareInterface(ros::NodeHandle& nh) : nh_(nh) {
        init();
        controller_manager_.reset(new controller_manager::ControllerManager(this, nh_));
        nh_.param("/rrbot/hardware_interface/loop_hz", loop_hz_, 0.1);  //Note: 0.1hz is the default value if loop_hz can't be found in the parameter server
        ros::Duration update_freq = ros::Duration(1.0/loop_hz_);
        non_realtime_loop_ = nh_.createTimer(update_freq, &rrbotHardwareInterface::update, this);
    }

    rrbotHardwareInterface::~rrbotHardwareInterface() {

    }

    void rrbotHardwareInterface::init() {
        // Get joint names
        nh_.getParam("/rrbot/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_ ...