simple hardware interface for mobile robot (RobAir)

Question

I want to create a hardware interface to interface a simple research mobile robot called RobAir, this robot has two wheels and using velocity controller cmd_vel through a ROS node on Arduino board to control he wheels.

What I have tried so far (full ROS package available here) is:

#include <chrono>
#include <functional>
#include <ros/callback_queue.h>
#include <robair_hw_iface.h>

void controlLoop(RobAirHWInterface &hw, controller_manager::ControllerManager &cm, std::chrono::system_clock::time_point &last_time)
{
    std::chrono::system_clock::time_point current_time = std::chrono::system_clock::now();
    std::chrono::duration<double> elapsed_time = current_time - last_time;
    ros::Duration elapsed(elapsed_time.count());
    last_time = current_time;

    hw.read(elapsed);
    cm.update(ros::Time::now(), elapsed);
    hw.write();
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "robair_base_node");

    RobAirHWInterface hw;
    controller_manager::ControllerManager cm(&hw, hw.nh);

    double control_frequency;
    hw.private_nh.param<double>("control_frequency", control_frequency, 10.0);

    ros::CallbackQueue robair_queue;
    ros::AsyncSpinner robair_spinner(1, &robair_queue);

    std::chrono::system_clock::time_point last_time = std::chrono::system_clock::now();
    ros::TimerOptions control_timer(
        ros::Duration(1 / control_frequency), 
        std::bind(controlLoop, std::ref(hw), std::ref(cm), std::ref(last_time)), 
        &robair_queue);
    ros::Timer control_loop = hw.nh.createTimer(control_timer);
    robair_spinner.start();
    ros::spin();

    return 0;
}

However, my implementation didn't work (I'm not able to control the wheels using my interface), can you please advise me on how to fix that? or guide me to a close implementation for another robot where I can get some hints? thanks in advance.

Answer 1

I'm having a hard time following your code. The biggest question I would have for you is this: How do you think you are sending a velocity command? The closest thing I can find is this line:

hardware_interface::JointHandle vel_handle(jnt_state_interface.getHandle(os.str()), &cmd[i]);

but honestly (1) it looks like it's reading values and putting them into the command, not sending them, and (2) if it is writing commands, then I don't see where a publisher is set or where you're calling spin_once() or a loop time or anything else like that.

I see where you have a wheel velocity and you're writing those, but I'm assuming they must be for information and not commands because those values are tied to encoder counts:

    double ang_distance_left = _wheel_angle[0];
    double ang_distance_right = _wheel_angle[1];
    pos[0] += ang_distance_left;
    vel[0] += ang_distance_left / period.toSec();
    pos[1] += ang_distance_right;
    vel[1] += ang_distance_right / period.toSec();

Please edit your question to explain your intended flow. Where are the speed commands coming from? How are they getting forwarded to speed controllers? How have you verified the speed commands are being published? How have you verified the speed controllers are functioning?

Answer 2

Are you limited to ROS, or could you upgrade to ROS 2? The documentation of ros2_control is in a more complete state than the older one.

I don't have a working ros_control setup as I'm working only on ROS 2, so I can't give you any reference implementations unfortunately.

When I understand your code, you use topics to exchange data from the hardware_interface and your arduino? Does it startup properly, can you receive/send topics?