ros2_control的简单应用

最新推荐文章于 2024-03-11 19:22:01 发布
最新推荐文章于 2024-03-11 19:22:01 发布
阅读量658 收藏 5
点赞数 1
分类专栏： ros 文章标签： ros2
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/joyopirate/article/details/131092613
版权

文章目录

简介

在利用moveit_setup_assistant配置我们自己机械手后，当运行demo.launch.py时，会实例化一个moveit对象以及一个基于ros2_control的、虚拟的控制对象，从而可以实现一个完整的控制闭环。
此基于ros2_control的虚拟对象，包含了action（server）相关的实例化、关节状态的发布、虚拟伺服电机的驱动及读取。
我们也可以利用ros2_control，将我们自己的机械手与moveit连接起来，从而实现moveit对我们机械手的控制。但是这样做的话，会损失比较多的自由度。假如电机等伺服机构、传感器等硬件是直接接到ros系统所在板子上的话，用ros2_control是挺方便的，但是我的是接在下位机，然后通过串口通讯控制的，使用ros2_control貌似就不太合适了。
不过也简单尝试一下例程，说不定后面有机会用到。

插件实现

这里我还是用我之前的那个【机械手模型】，来进行演示。

关于ros2_control的介绍可以看这里：【ros2_control doc】，我们这里就不继续啰嗦了。
我们只需要知道一点够了：我们只需要对ros2_control的类hardware_interface::SystemInterface进行实例化，其余的moveit_setup_assistant/ros2_control已经帮我们做好了。
对该类进行重写实现时，根据官方文档【Writing a new hardware interface】，我们需要分别重写下面8个函数

    hardware_interface::CallbackReturn on_init(
        const hardware_interface::HardwareInfo & info) override;

    hardware_interface::CallbackReturn on_configure(
        const rclcpp_lifecycle::State & previous_state) override;

    std::vector<hardware_interface::StateInterface> export_state_interfaces() override;

    std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;

    hardware_interface::CallbackReturn on_activate(
        const rclcpp_lifecycle::State & previous_state) override;
        
    hardware_interface::CallbackReturn on_deactivate(
        const rclcpp_lifecycle::State & previous_state) override;

    hardware_interface::return_type read(
        const rclcpp::Time & time, const rclcpp::Duration & period) override;

    hardware_interface::return_type write(
        const rclcpp::Time & time, const rclcpp::Duration & period) override;

函数介绍

分别介绍一下这几个函数的作用。

  1. hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override;
    在这个函数内要进行软件方面的初始化、检查等。主要是对将要用到的state、command的缓存空间进行申请、检查urdf规定的接口是不是符合我们的需求等。
  1. hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override;
    在这个函数内，要进行硬件的连接，成功与其进行了通讯，并进行一些必要的配置（通讯频率、限制等？）
  1. std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
    这个函数主要是将在on_init里面申请好的state存储地址打包，返回给上层操作者
  1. std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
    这个函数主要是将在on_init里面申请好的command内存地址打包，返回给上层操作者
  1. hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
    在这个函数内，要让执行机构进行复位、清除异常等操作，准备好接收运动指令。
  1. hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
    在这个函数内，要让执行机构进行断电、断气、解除负载等操作？
  1. hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
    读取。从硬件读取此时位置，放到缓存中，再由上层来读取
  1. hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
    写入。将从上层得到的指令（目标位置）发送给硬件

代码

myrobotinterface.h

#ifndef MYROBOTINTERFACE_H
#define MYROBOTINTERFACE_H

#include <memory>
#include <string>
#include <vector>

#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"

class MyRobotInterface : public hardware_interface::SystemInterface
{

public:
    // 这代码的作用是使此类能够直接使用 MyRobotInterface::sharedPtr
    RCLCPP_SHARED_PTR_DEFINITIONS(MyRobotInterface);

    MyRobotInterface();

    // 初始化时的函数
    hardware_interface::CallbackReturn on_init(
        const hardware_interface::HardwareInfo & info) override;

    // 配置时的函数
    hardware_interface::CallbackReturn on_configure(
        const rclcpp_lifecycle::State & previous_state) override;

    // 导出状态数据的存储地址
    std::vector<hardware_interface::StateInterface> export_state_interfaces() override;

    // 导出命令数据的存储地址
    std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;

    // 激活时的函数
    hardware_interface::CallbackReturn on_activate(
        const rclcpp_lifecycle::State & previous_state) override;

    // 取消激活时的函数
    hardware_interface::CallbackReturn on_deactivate(
        const rclcpp_lifecycle::State & previous_state) override;

    // 读取，从硬件读取此时位置
    hardware_interface::return_type read(
        const rclcpp::Time & time, const rclcpp::Duration & period) override;

    // 写入，将指令发送给硬件
    hardware_interface::return_type write(
        const rclcpp::Time & time, const rclcpp::Duration & period) override;

private:
    // Parameters for the RRBot simulation
    // 用于模拟的参数
    double hw_start_sec_;
    double hw_stop_sec_;
    double hw_slowdown_;

    // Store the command for the simulated robot
    // 为此模拟机器人存储命令
    std::vector<double> hw_commands_;

    // 用于存储每个joint的状态。假如都是旋转关节，那么存储的就是当前的角度
    std::vector<double> hw_states_;

};

#endif // MYROBOTINTERFACE_H

myrobotinterface.cpp

#include "myrobotinterface.h"

MyRobotInterface::MyRobotInterface()
{

}

hardware_interface::CallbackReturn MyRobotInterface::on_init(const hardware_interface::HardwareInfo &info)
{
    // 这个是初始化，但是是偏向于软件方面的初始化；比如ros2_controllers.yaml里面描述的接口是否有问题，一些人为设定的其他参数等等

    // 先让hardware_interface::HardwareInfo执行初始化（也就是赋值info_），它初始化成功了，我们自己再进行自己的初始化
    if (
        hardware_interface::SystemInterface::on_init(info) !=
        hardware_interface::CallbackReturn::SUCCESS)
    {
        return hardware_interface::CallbackReturn::ERROR;
    }

    // 上面经过 hardware_interface::SystemInterface::on_init 之后，info_已经被初始化了，可以直接用
    // stod: string to double

    // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
    hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
    hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]);
    hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]);

    // END: This part here is for exemplary purposes - Please do not copy to your production code
    hw_states_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
    hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());

    // 遍历urdf文件所描述的所有joint，这里是和外面 ros2_controllers.yaml 里面的controller对应的
    // command指的是moveit发送过来的指令，也就是本关节需要走到哪个角度；state指的是当前关节的状态
    for (const hardware_interface::ComponentInfo & joint : info_.joints)
    {
        // 每个joint只接受1个指令
        // RRBotSystemPositionOnly has exactly one state and command interface on each joint
        if (joint.command_interfaces.size() != 1)
        {
            RCLCPP_FATAL(
                rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
                "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(),
                joint.command_interfaces.size());
            return hardware_interface::CallbackReturn::ERROR;
        }

        // 只接受 position类型的指令
        if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
        {
            RCLCPP_FATAL(
                rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
                "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
                joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
            return hardware_interface::CallbackReturn::ERROR;
        }

        // 只接受一种状态
        if (joint.state_interfaces.size() != 1)
        {
            RCLCPP_FATAL(
                rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
                "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(),
                joint.state_interfaces.size());
            return hardware_interface::CallbackReturn::ERROR;
        }

        // 只接受position状态
        if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION)
        {
            RCLCPP_FATAL(
                rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
                "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(),
                joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
            return hardware_interface::CallbackReturn::ERROR;
        }
    }

    return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn MyRobotInterface::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
{
    // 这里的函数是进行配置的，是经过init之后，再到这一步
    // 这个也是初始化，但是这个会偏向硬件方面，主要是启动设备、检查设备等

    // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
    RCLCPP_INFO(
        rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Configuring ...please wait...");

    // 这里根据前面设定的启动时间，在模拟启动耗时
    for (int i = 0; i < hw_start_sec_; i++)
    {
        rclcpp::sleep_for(std::chrono::seconds(1));
        RCLCPP_INFO(
            rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...",
            hw_start_sec_ - i);
    }
    // END: This part here is for exemplary purposes - Please do not copy to your production code

    // 对状态、命令等模拟参数进行初始化
    // reset values always when configuring hardware
    for (uint i = 0; i < hw_states_.size(); i++)
    {
        hw_states_[i] = 0;
        hw_commands_[i] = 0;
    }

    RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully configured!");

    return hardware_interface::CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::StateInterface> MyRobotInterface::export_state_interfaces()
{
    // 这个是此机器人提供关节当前状态的数据存储地址，以供上层应用查询
    // 上层查询时，直接通过此次获取到的地址读取数值即可，无需重复调用函数

    std::vector<hardware_interface::StateInterface> state_interfaces;
    for (uint i = 0; i < info_.joints.size(); i++)
    {
        state_interfaces.emplace_back(hardware_interface::StateInterface(
            info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i]));
    }

    return state_interfaces;
}

std::vector<hardware_interface::CommandInterface> MyRobotInterface::export_command_interfaces()
{
    // 这个是此机器人提供关节当前正在执行的命令的存储地址，以供上层读写

    std::vector<hardware_interface::CommandInterface> command_interfaces;
    for (uint i = 0; i < info_.joints.size(); i++)
    {
        command_interfaces.emplace_back(hardware_interface::CommandInterface(
            info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
    }

    return command_interfaces;
}

hardware_interface::CallbackReturn MyRobotInterface::on_activate(const rclcpp_lifecycle::State &/*previous_state*/)
{
    // 设备开始时执行此函数
    // 这个activate。。。，怎么感觉和前面的 on_configure 一样的作用？

    // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
    RCLCPP_INFO(
        rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Activating ...please wait...");

    for (int i = 0; i < hw_start_sec_; i++)
    {
        rclcpp::sleep_for(std::chrono::seconds(1));
        RCLCPP_INFO(
            rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...",
            hw_start_sec_ - i);
    }
    // END: This part here is for exemplary purposes - Please do not copy to your production code

    // 在激活时，目标位置和当前位置应该要一样？
    // command and state should be equal when starting
    for (uint i = 0; i < hw_states_.size(); i++)
    {
        hw_commands_[i] = hw_states_[i];
    }

    RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully activated!");

    return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn MyRobotInterface::on_deactivate(const rclcpp_lifecycle::State &/*previous_state*/)
{
    // 设备停止时执行此函数

    // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
    RCLCPP_INFO(
        rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Deactivating ...please wait...");

    for (int i = 0; i < hw_stop_sec_; i++)
    {
        rclcpp::sleep_for(std::chrono::seconds(1));
        RCLCPP_INFO(
            rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...",
            hw_stop_sec_ - i);
    }

    RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully deactivated!");
    // END: This part here is for exemplary purposes - Please do not copy to your production code

    return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::return_type MyRobotInterface::read(const rclcpp::Time &time, const rclcpp::Duration &period)
{
    // 这个是进行执行器关节状态的读取，将读取到的值存放在hw_states_上面。相当于从硬件同步数据

    // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
    RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Reading...");

    for (uint i = 0; i < hw_states_.size(); i++)
    {
        // Simulate RRBot's movement
        hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_;
        RCLCPP_INFO(
            rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got state %.5f for joint %d!",
            hw_states_[i], i);
    }
    RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully read!");
    // END: This part here is for exemplary purposes - Please do not copy to your production code

    return hardware_interface::return_type::OK;
}

hardware_interface::return_type MyRobotInterface::write(const rclcpp::Time &time, const rclcpp::Duration &period)
{
    // 这个是将需要发送的目标位置写到执行器。
    // 需要写入（执行)的值已经被上层写到 hw_commands_ 了,所以在此处传入进来的参数中，并没看到具体关节的值
    

    // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
    RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Writing...");

    for (uint i = 0; i < hw_commands_.size(); i++)
    {
        // Simulate sending commands to the hardware
        RCLCPP_INFO(
            rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got command %.5f for joint %d!",
            hw_commands_[i], i);
    }
    RCLCPP_INFO(
        rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully written!");
    // END: This part here is for exemplary purposes - Please do not copy to your production code

    return hardware_interface::return_type::OK;
}

调用原理

需要注意的是，此实例化的类是以插件的形式被ros2_control来调用的(In ros2_control hardware system components are libraries, dynamically loaded by the controller manager using the pluginlib interface. )，也就是我们urdf文件中的ros2_control–》hardware–》plugin节点中指定的插件。
在这里插入图片描述比如说上面的 mock_components/GenericSystem，我们可以在/opt/ros/humble/share/hardware_interface、/opt/ros/humble/include/mock_components找到他们的踪迹。
在这里插入图片描述因此，我们需要把我们的硬件接口编译成动态库，然后填写一个插件描述文件放在文件夹中，然后ros2_control通过名字来找到此插件。

局限性

这样一来有个问题，我希望Action的Server接口是由我们的Qt程序来实现，与我们的Qt程序处于同一个进程，而不是一个插件、动态库的形式，因为我需要取得路径点进行调试、与其他类操作进行交互等，这该如何处理？

可能这个ros2_control，是比较适合组件化、黑盒子的设计吧，而不是我目前需要弄的一体化设计。
暂时不研究这个了。

参考：
【 ros-controls/ros2_control_demos】

韭菜钟
关注
  • 1
    点赞
  • 踩
  • 5
    收藏
    觉得还不错? 一键收藏
  • 3
    评论
专栏目录
ros2_android_examples:用于ROS2 Java绑定的示例Android应用
05-13
ROS2 Android示例应用程序 这是使用ROS2使用Android / Java绑定的示例的集合 基于rclandroid ： ros2_android_listener ros2_android_talker 基于rcljava ： ros2_listener_android ros2_talker_android 2016年10月3日从
ROS2自学实践(14)：ROS Control简介与使用
qq_43066145的博客
02-03 3687
ROS Controller
ROS控制机械臂【2】：ros_control与实现
Rot_Tianers
06-18 5059
ros_congrol与实现
ROS2与gazebo联合仿真自定义机械臂（ros2_control）
最新发布
ZLW07的博客
03-11 485
type: position_controllers/JointGroupPositionController // ros2_control 里面定义的控制器type: joint_state_broadcaster/JointStateBroadcaster //固定的joints:- Joint_1 // 注意和机械臂的urdf中定义的关节角保持一致- Joint_2- Joint_3- Joint_4- Joint_5- Joint_6- position- position。
通过ROS控制真实机械臂(2)----单轴运动，手柄控制
工大机器人工作室
07-14 9091
创建ROS包，包名redwall_arm ，通过自定义的消息，将手柄的数据发布 msg/ joycontrol.msg，内容如下，分别对应罗技手柄的按钮和遥杆轴。 int32 button1 int32 button2 int32 button3 int32 button4 int32 button5 int32 button6 int32 button7 int32 button8 floa...
ROS 2 驱动程序现在可用于 ABB 的机械臂
zhangrelay的专栏
06-21 1680
2021年：ABB之ROS功能更新2021_zhangrelay的博客-CSDN博客PickNik Robotics 和 Optimax Systems 发布了用于ABB 机械手的ROS 2 驱动程序。该包装已使用 ABB IRB 1200-5/0.9 机械手进行了测试。开发人员说“移植新的机器人模型应该是一个简单的过程，我们欢迎拉取请求。”新的 ABB 软件包允许两种模拟模式。在第一种模式下，它可以欺骗 ros2_control 驱动程序以在 ROS 中进行仿真。或者，驱动程序可以与 ABB 专有的 Ro
配置 ros2_control 在Gazebo控制关节运动
mubibaiwhale的博客
07-15 1328
接上文在ROS2使用Gazebo控制小车，上文控制小车使用的是libgazebo_ros_diff_drive.so这个插件，输入模型的数据就可以对小车进行差速控制。本文介绍另外一种控制方式ros2_control。
ros2_control中的controller manager 和 Nav2 中的 lifecycle_manager
mubibaiwhale的博客
07-16 461
这一节补一下前面填的坑，介绍一下ros2_control中的controller manager 和 Nav2 中的 lifecycle_manager。
ros2_control官方文档
生活不止眼前的苟且
10-10 1362
文章信息：标题：ros2_control官方文档原文地址：https://fishros.org.cn/forum/api/topic/12关键词:,在线文档,参与者:,版权声明: 文章中所有知识产权归鱼香ROS及原作者所有。
ROS知识：硬件和控制：ROS-Control
gongdiwudu的专栏
09-17 4353
app与hardware之间是如何进行运作的？是通过ros-control进行的。即app向control发出topic；然后ros-control进行hardware传递信息。事实上，是上位机和下位机通过ros-control沟通。 因此，app发送给ros-control后，并不能真正控制，而是通过下位机app进行的。 ...
ROS2使用Gazebo控制AGV
mubibaiwhale的博客
07-08 1820
目前国内关于ROS2的教程比较少，我记录一下再ROS2中使用Gazebo进行仿真的过程，也算是总结这一段时间关于ROS2的学习。首先做的项目就是用控制小车运动，
ros_control:ROS的通用和简单控件框架
05-13
ros_control 请参阅上的ros_control 建置状态 靛青 动能 月球 旋律 理性的 基于源的安装的分支 ROS靛蓝 ROS动力学 ROS月球 ROS旋律 ROS Noetic 靛蓝开发 动能发展 动能发展 旋律发展 新奇发展 出版物 如果您认为这项工作有用，请通过引用以下方式来感谢作者： S. Chitta，E.黄鼠狼-Eppstein的，W. Meeussen，V.与Pradeep，A.罗德里格斯Tsouroukdissian，J. Bohren，D.科尔曼，B.匈牙利，G.莱奥拉，M.Lüdtke和E.费尔南德斯Perl多莫“ros_control： ROS的通用和简单控制框架” ，《开源软件杂志》，2017年。（ ） @article{ros_control, author = {Chitta, Sachin and Marder-Eppstein, Eit
ros2_android:ros2java的Android库
05-12
ROS2 Android核心 这是ROS2的Android核心绑定 更多文档
ros_control_boilerplate:提供简单的仿真界面和模板，用于为ros_control设置硬件界面
05-14
该样板演示： 创建用于与ros_control一起使用的多个关节的hardware_interface 位置轨迹控制器简单机器人“ RRBot”的2个关节的控制如下图所示使用roslaunch和yaml文件加载配置生成随机轨迹并将其通过actionlib接口...
ros2_control的ODrive驱动程序C++_Python_下载
08-31
odrive_ros2_control\nENGLISH / 中文\n介绍\nros2_control 的 ODrive 驱动程序\n兼容性\nROS 2 狡猾的菲茨罗伊 ROS 2 谦逊的玳瑁\nODrive 固件 v0.5.3 狡猾的fw-v0.5.3 谦虚-fw-v0.5.3\nODrive 固件 v0.5.1 狡猾的...
ROS自学实践(15)：ROS2控制仿真车辆
qq_43066145的博客
02-04 1647
ros_controller
ROS2 Control hardware_interface说明
机器人梦想家 Bing
03-05 1218
hardware_interface是ROS control控制使用的硬件接口，用来向硬件发送 ( write() ) 命令并从其接收 ( read() ) 各轴的状态。本文主要对ROS control常用的hardware_interface做了整理汇总统一说明，方便后续查阅了解使用。
ROS中的机器人模型控制——ros_control
m0_57368670的博客
10-19 1835
ros中的机器人模型控制——ros_control
松灵机器人——ROS下的CAN通讯调试（内含所需调用包，都是操作步骤）
weixin_48924581的博客
12-09 7380
松灵机器人——ROS下的CAN通讯调试（都是操作步骤） 1 代码包地址 scout_ros package：https://github.com/westonrobot/scout_ros.git ugv_sdk-main package：https://github.com/westonrobot/ugv_sdk webots package：https://cyberbotics.com/ 2 硬件准备 2.1 SCOUT MINI 小白车 + 尾部航空插头 + CAN_TO_USB适配器
ros中PID算法python接口
07-11
在ROS中，可以使用Python编写PID算法的接口。下面是一个简单的示例代码，演示如何在ROS中使用PID算法： ```python #!/usr/bin/env python import rospy from std_msgs.msg import Float64 from geometry_msgs.msg import Twist class PIDController: def __init__(self, Kp, Ki, Kd): self.Kp = Kp self.Ki = Ki self.Kd = Kd self.error_integral = 0.0 self.last_error = 0.0 def compute_control(self, error): self.error_integral += error error_derivative = error - self.last_error self.last_error = error control = self.Kp * error + self.Ki * self.error_integral + self.Kd * error_derivative return control def callback(data): # 设置PID参数 Kp = 1.0 Ki = 0.5 Kd = 0.1 # 创建PID控制器对象 pid_controller = PIDController(Kp, Ki, Kd) # 获取当前位置信息 current_position = data.linear.x # 设置目标位置为0.0 target_position = 0.0 # 计算误差 error = target_position - current_position # 使用PID控制器计算控制量 control = pid_controller.compute_control(error) # 发布控制量到机器人 pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) twist = Twist() twist.linear.x = control pub.publish(twist) def pid_controller(): rospy.init_node('pid_controller', anonymous=True) rospy.Subscriber('/current_position', Float64, callback) rospy.spin() if __name__ == '__main__': pid_controller() ``` 这段代码定义了一个PIDController类，其中包含了计算控制量的方法compute_control。在callback函数中，我们可以获取当前位置信息，并使用PID控制器计算控制量，最后将控制量发布到`/cmd_vel`话题上。 请注意，这只是一个简单的示例，实际应用中可能需要根据具体情况进行修改。你可以根据你的需求来调整PID参数和订阅的话题以及发布的话题。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 3
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值