ROS控制器

Minimal Simulator

#include<ros/ros.h> 
#include<std_msgs/Float64.h> 
std_msgs::Float64 g_velocity;
std_msgs::Float64 g_force;

void myCallback(const std_msgs::Float64& message_holder) {
    ROS_INFO("received force value is: %f", message_holder.data);
    g_force.data = message_holder.data; 
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "minimal_simulator");
    ros::NodeHandle nh; 
    ros::Subscriber my_subscriber_object = nh.subscribe("force_cmd", 1, myCallback);
    ros::Publisher my_publisher_object = nh.advertise<std_msgs::Float64>("velocity", 1);
    // 订阅力控制话题，通过积分发布速度话题

    double mass = 1.0;
    double dt = 0.01;   //10ms 积分时间
    double sample_rate = 1.0 / dt; 
    ros::Rate naptime(sample_rate);
    g_velocity.data = 0.0;  
    g_force.data = 0.0; 

    while (ros::ok()) {
        g_velocity.data = g_velocity.data + (g_force.data / mass) * dt; 
        my_publisher_object.publish(g_velocity); 
        ROS_INFO("velocity = %f", g_velocity.data);
        ros::spinOnce();
        naptime.sleep(); 
    }
    return 0;
} 

Minimal Controller

#include <ros/ros.h>
#include <std_msgs/Float64.h>

std_msgs::Float64 g_vel;
std_msgs::Float64 g_vel_cmd;
std_msgs::Float64 g_force;

void vel_cb(const std_msgs::Float64::ConstPtr &velocity)
{
    ROS_WARN("Receive velocity: %f m/s", velocity->data);
    g_vel.data = velocity->data;
}

void vel_cmd_cb(const std_msgs::Float64::ConstPtr &vel_cmd)
{
    ROS_WARN("Receive desired velocity: %f m/s", vel_cmd->data);
    g_vel_cmd.data = vel_cmd->data;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "controller");
    ros::NodeHandle nh;
    // 订阅当前速度和期望速度
    ros::Subscriber vel_sub = nh.subscribe("velocity", 1, vel_cb);
    ros::Subscriber vel_cmd_sub = nh.subscribe("vel_cmd", 1, vel_cmd_cb);

    // 输出期望控制力
    ros::Publisher force_pub = nh.advertise<std_msgs::Float64>("force_cmd", 10);
    double Kv = 1.0;    // 比例控制器
    double dt = 0.01; 
    double sample_rate = 1.0 / dt; 
    ros::Rate naptime(sample_rate);
    g_vel.data = 0.0; 
    g_vel_cmd.data = 0.0; 
    g_force.data = 0.0;
    double vel_err = 0.0;
    while(ros::ok())
    {
        vel_err = g_vel_cmd.data - g_vel.data;
        g_force.data = Kv * vel_err;
        force_pub.publish(g_force);
        ROS_INFO("force command = %f N", g_force.data);
        ros::spinOnce();
        naptime.sleep();
    }
}

launch文件

<launch>
    <node name="simulator" pkg="ros_learning" type="simulator_node"/>
    <node name="controller" pkg="ros_learning" type="controller_node"/>
</launch>
# name: 节点名
# pkg: 包名
# type: 可执行文件名

rqt_plot 画曲线

rostopic pub -r 10 vel_cmd std_msgs/Float64 0.5
rqt_plot velocity/data, vel_cmd/data, force_cmd/data

profiles