鱼香ROS社区
    • 版块
    • 最新
    • 未解决
    • 已解决
    • 群组
    • 注册
    • 登录
    紧急通知:禁止一切关于政治&VPN翻墙等话题,发现相关帖子会立马删除封号
    提问前必看的发帖注意事项: 社区问答规则(小鱼个人)更新 | 高质量帖子发布指南

    求助--底盘控制板代码谁能帮忙改一下?

    已定时 已固定 已锁定 已移动
    移动平台分享
    求助贴 装机实战求助
    2
    3
    471
    正在加载更多帖子
    • 从旧到新
    • 从新到旧
    • 最多赞同
    回复
    • 在新帖中回复
    登录后回复
    此主题已被删除。只有拥有主题管理权限的用户可以查看。
    • 毛哥成山轮胎机油保养毛
      毛哥成山轮胎机油保养 活跃VIP
      最后由 毛哥成山轮胎机油保养 编辑

      底盘用键盘控制能动了,控制话题/cmd_vel 速度话题/raw_vel,雷达能提供数据了,话题/scan,怎样把他们组成一个有机的整体呢,现在只是一堆零件安装在了一起,需要修改代码

      毛哥成山轮胎机油保养毛 1 条回复 最后回复 回复 引用 0
      • 毛哥成山轮胎机油保养毛
        毛哥成山轮胎机油保养 活跃VIP @毛哥成山轮胎机油保养
        最后由 毛哥成山轮胎机油保养 编辑

        控制板实物图
        IMG_20220713_160404.jpg
        此控制板提供2个话题:
        速度话题/raw_vel 是实时的线速度和角速度,需要转换成odom话题,懂得帮忙给改一下
        控制话题/cmd_vel 通过键盘控制节点可以前后左右移动这是标准的ros2控制话题,不用改。

        底盘控制板代码目录结构
        7b1c2679-babd-4136-9fc9-04b39a9ff800-image.png

        CMakeLists.txt

        cmake_minimum_required(VERSION 3.5)
        project(rikibot_foc_driver)
        
        #Default to C++14
        if(NOT CMAKE_CXX_STANDARD)
          set(CMAKE_CXX_STANDARD 14)
        endif()
        
        if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
          add_compile_options(-Wall -Wextra -Wpedantic)
        endif()
        
        include_directories(include
        	            ${Boost_INCLUDE_DIRS})
        
        find_package(ament_cmake REQUIRED)
        find_package(rclcpp REQUIRED)
        find_package(std_msgs REQUIRED)
        find_package(geometry_msgs REQUIRED)
        find_package(nav_msgs REQUIRED)
        find_package(tf2 REQUIRED)
        find_package(riki_msgs REQUIRED)
        find_package(serial REQUIRED)
        find_package(tf2_ros REQUIRED)
        find_package(Boost REQUIRED COMPONENTS system filesystem thread program_options)
        
        add_executable(rikibot_foc_driver src/rikibot_foc_driver.cpp)
        target_link_libraries(rikibot_foc_driver
        	              ${Boost_LIBRARIES})
        ament_target_dependencies(rikibot_foc_driver 
        			  rclcpp 
                      geometry_msgs
                      nav_msgs
                      tf2
                      tf2_ros
        			  std_msgs
                      serial
                      riki_msgs)
        
        install(TARGETS
          rikibot_foc_driver
          DESTINATION lib/${PROJECT_NAME})
        
        install(
          DIRECTORY include/
          DESTINATION include)
        
        install(
          DIRECTORY launch
          DESTINATION share/${PROJECT_NAME})
        
        
        ament_package()
        
        

        rikibot_foc_driver.h

        #include <chrono>
        #include <memory>
        
        #include "rclcpp/rclcpp.hpp"
        #include "std_msgs/msg/string.hpp"
        
        #include <nav_msgs/msg/odometry.hpp>
        #include <geometry_msgs/msg/twist.hpp>
        //#include <tf/tf.h>
        #include <riki_msgs/msg/velocities.hpp>
        #include <geometry_msgs/msg/vector3.hpp>
        #include <std_msgs/msg/float32_multi_array.hpp>
        #include <riki_msgs/msg/imu.hpp>
        #include <riki_msgs/msg/battery.hpp>
        
        #include <string>
        #include <vector>
        #include <math.h>
        #include <serial/serial.h>
        
        
        #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
        
        #define BATTERY_RATE 1
        
        #define START_FRAME 0xABCD
        
        #define MAX_RPM   	366
        #define FOC_MAX_PWM     1000
        #define WHEEL_DIAMETER  0.165
        
        #define LR_WHEELS_DISTANCE 0.33
        #define FR_WHEELS_DISTANCE 0.0
        #define PI                 3.1415926
        
        typedef struct{
            uint16_t  start;          //0XABCD
            int16_t   rpmR;    	      //右轮转速
            int16_t   rpmL;    	      //左轮转速
            int16_t   batVoltage;     //主板电压
            int16_t   boardTemp;      //主板温度
            int16_t   curL_DC;        //左电机电流
            int16_t   curR_DC;        //右电机电流
            uint16_t  checksum;       //校验和
        } RikibotFeedback;
        
        typedef struct{
           uint16_t start;
           int16_t  mSpeedR;
           int16_t  mSpeedL;
           uint16_t checksum;
        } RikibotCommand;
        
        typedef struct{
          int motor1;
          int motor2;
        }MotorRPM;
        
        typedef struct {
          float linear_x;
          float linear_y;
          float angular_z;
        }Velocities;
        
        
        class RikibotFocDriver: public rclcpp::Node{
            public:
                RikibotFocDriver();
                ~RikibotFocDriver();
                void loop();
            
            private:
        
        	    rclcpp::TimerBase::SharedPtr topic_timer_;
        	    rclcpp::Publisher<riki_msgs::msg::Velocities>::SharedPtr raw_vel_pub_;
        	    rclcpp::Publisher<riki_msgs::msg::Battery>::SharedPtr battery_pub_;
                rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr velocity_sub_;
                bool ReadFormSerial();
        
                void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr twist_msg);
                void RosNodeTopicCallback();
                void SetVelocity(double x, double y, double angular_z);
        
                void calculateRPM(float linear_x, float linear_y, float angular_z);
                void getVelocities(int rpm1, int rpm2);
        	    int16_t map(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max);
                void PublisherMotorVel();
                void publisherBattery();
        
                int max_rpm_;
        	    float wheel_diameter_;
        	    float wheels_x_distance_;
        	    float wheels_y_distance_;
        	    float wheel_circumference_;
        
                serial::Serial Robot_Serial;
        	
        	    RikibotFeedback Feedback;        
                RikibotCommand  Command;
                Velocities vel;
                MotorRPM req_rpm;
        
                rclcpp::Time now_;
        
                riki_msgs::msg::Velocities raw_vel_msg;
                riki_msgs::msg::Battery raw_battery_msg;
        
                std::string port_name_;
                std::string rxdata;
        
                 
                int baud_rate_;
        
                int control_rate_;
        
        };
        
        
        
        

        rikibot_foc_driver.launch.py

        import os
        import launch
        import launch.actions
        import launch.substitutions
        import launch_ros.actions
        from ament_index_python.packages import get_package_share_directory
        
        
        def generate_launch_description():
            launch_description = launch.LaunchDescription()
            launch_description.add_action(
                launch_ros.actions.Node(
                    package='rikibot_foc_driver', 
                    node_executable='rikibot_foc_driver',
                    name='rikibot_foc_driver', 
                    output='screen',)
            )
            return launch_description
        
        

        package.xml

        <?xml version="1.0"?>
        <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
        <package format="3">
          <name>rikibot_foc_driver</name>
          <version>0.0.0</version>
          <description>TODO: Package description</description>
          <maintainer email="304050118@qq.com">rikibot</maintainer>
          <license>TODO: License declaration</license>
        
          <buildtool_depend>ament_cmake</buildtool_depend>
          <depend>rclcpp</depend>
          <depend>std_msgs</depend>
          <depend>std_srvs</depend>
          <depend>geometry_msgs</depend>
          <depend>nav_msgs</depend>
          <depend>riki_msgs</depend>
          <depend>tf2</depend>
          <depend>tf2_ros</depend>
        
          <test_depend>ament_lint_auto</test_depend>
          <test_depend>ament_lint_common</test_depend>
        
          <export>
            <build_type>ament_cmake</build_type>
          </export>
        </package>
        
        

        rikibot_foc_driver.cpp

        //#include <chrono>
        //#include <memory>
        
        //#include "rclcpp/rclcpp.hpp"
        //#include "std_msgs/msg/string.hpp"
        #include "rikibot_foc_driver/rikibot_foc_driver.h"
        
        
        using namespace std::chrono_literals;
        
        
        RikibotFocDriver::RikibotFocDriver(): Node("rikibot_foc_driver")
        {
          wheel_diameter_ = WHEEL_DIAMETER;
          wheel_circumference_ = PI * wheel_diameter_;
          max_rpm_ = MAX_RPM;
          wheels_x_distance_ = FR_WHEELS_DISTANCE;
          wheels_y_distance_ = LR_WHEELS_DISTANCE;
          now_ = now();
        
          declare_parameter("port_name", std::string("/dev/ttyTHS1"));
          get_parameter("port_name", port_name_);
          declare_parameter("baud_rate", 115200);
          get_parameter("baud_rate", baud_rate_);
        
          raw_vel_pub_ = this->create_publisher<riki_msgs::msg::Velocities>("raw_vel", 10);
          battery_pub_ = this->create_publisher<riki_msgs::msg::Battery>("battery", 10);
        
          topic_timer_ = this->create_wall_timer(10ms, std::bind(&RikibotFocDriver::RosNodeTopicCallback, this));
        
          velocity_sub_ = this->create_subscription<geometry_msgs::msg::Twist>("cmd_vel", 10, std::bind(&RikibotFocDriver::cmd_vel_callback, this, std::placeholders::_1));
        
        
          /**open serial device**/
          try{
              Robot_Serial.setPort(port_name_);
              Robot_Serial.setBaudrate(baud_rate_);
              serial::Timeout to = serial::Timeout::simpleTimeout(2000);
              Robot_Serial.setTimeout(to);
              Robot_Serial.open();
          }catch (serial::IOException& e){
              RCLCPP_INFO(this->get_logger(), "Rikibot Serial Unable to open port");
          }
        
          if(Robot_Serial.isOpen()){
              RCLCPP_INFO(this->get_logger(), "Rikibot Serial Port opened");
          }else{
          }
        
        }
        
        RikibotFocDriver::~RikibotFocDriver()
        {
          Robot_Serial.close();
        }
        
        void RikibotFocDriver::RosNodeTopicCallback()
        {
            if (true == ReadFormSerial()){
                PublisherMotorVel();
                publisherBattery();
            }
             //RCLCPP_INFO(this->get_logger(), "Hello from ROS2");
        }
        
        
        bool RikibotFocDriver::ReadFormSerial()
        {
            if(Robot_Serial.available()){
                rxdata = Robot_Serial.read(Robot_Serial.available());
                if (rxdata.size()==sizeof(RikibotFeedback)){
                    memcpy(&Feedback, rxdata.c_str(), sizeof(RikibotFeedback));
                    uint16_t checksum = (uint16_t)(Feedback.start^Feedback.rpmR^Feedback.rpmL^Feedback.batVoltage^Feedback.boardTemp^Feedback.curL_DC^Feedback.curR_DC);
                    if((Feedback.start == START_FRAME) && (checksum == Feedback.checksum))
                            return true;
                    else
                            return false;
                }else{
                    return false;
                }
            }else{
                return false;
            }
        }
        
        
        /*cmd_vel Subscriber的回调函数*/
        void RikibotFocDriver::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr twist_msg)
        {
            SetVelocity(twist_msg->linear.x, twist_msg->linear.y, twist_msg->angular.z);
        }
        
        
        /*底盘速度发送函数*/
        void RikibotFocDriver::SetVelocity(double x, double y, double angular_z)
        {
            calculateRPM(x, y, angular_z);
            int16_t mSpeedL = map(req_rpm.motor1, -max_rpm_, max_rpm_, -FOC_MAX_PWM,FOC_MAX_PWM);
            int16_t mSpeedR = map(req_rpm.motor2, -max_rpm_, max_rpm_, -FOC_MAX_PWM,FOC_MAX_PWM);
            Command.start = (uint16_t)START_FRAME;
            Command.mSpeedR = (uint16_t)mSpeedR;
            Command.mSpeedL = (uint16_t)mSpeedL;
            //printf("left rpm: mSpeedL: %d, rrpm: %d\r\n", mSpeedL, mSpeedR);
            Command.checksum = (uint16_t)(Command.start^Command.mSpeedR^Command.mSpeedL);
            Robot_Serial.write((uint8_t *)&Command, sizeof(Command));
        }
        
        
        void RikibotFocDriver::PublisherMotorVel()
        {
            getVelocities(Feedback.rpmL, Feedback.rpmR);
            raw_vel_msg.linear_x =  vel.linear_x;
            raw_vel_msg.linear_y  = 0;
            raw_vel_msg.angular_z =  vel.angular_z;
            raw_vel_pub_->publish(raw_vel_msg);
        }
        
        void RikibotFocDriver::publisherBattery()
        {
            if((now() - now_).seconds() > 1/BATTERY_RATE){
                raw_battery_msg.battery = (float)Feedback.batVoltage/100;
                if (raw_battery_msg.battery != 0){
                    battery_pub_->publish(raw_battery_msg);
                    now_ = now();
                }
            }
        }
        
        
        void RikibotFocDriver::calculateRPM(float linear_x, float linear_y, float angular_z)
        {
            float linear_vel_x_mins;
            float linear_vel_y_mins;
            float angular_vel_z_mins;
            float tangential_vel;
            float x_rpm;
            float y_rpm;
            float tan_rpm;
        
            //convert m/s to m/min
            linear_vel_x_mins = linear_x * 60;
            linear_vel_y_mins = linear_y * 60;
        
            //convert rad/s to rad/min
            angular_vel_z_mins = angular_z * 60;
        
            tangential_vel = angular_vel_z_mins * ((wheels_x_distance_ / 2) + (wheels_y_distance_ / 2));
        
            x_rpm = linear_vel_x_mins / wheel_circumference_;
            y_rpm = linear_vel_y_mins / wheel_circumference_;
            tan_rpm = tangential_vel / wheel_circumference_;
        
            req_rpm.motor1 = int16_t(x_rpm - y_rpm - tan_rpm);
            req_rpm.motor1 = constrain(req_rpm.motor1, -max_rpm_, max_rpm_);
            //front-right motor
            req_rpm.motor2 = -int16_t(x_rpm + y_rpm + tan_rpm);
            req_rpm.motor2 = constrain(req_rpm.motor2, -max_rpm_, max_rpm_);
        
        }
        
        void RikibotFocDriver::getVelocities(int rpm1, int rpm2 )
        {
            float average_rps_x;
            //float average_rps_y;
            float average_rps_a;
        
            //convert average revolutions per minute to revolutions per second
            average_rps_x = ((float)(rpm1 - rpm2) / 2) / 60;  // RPM
            vel.linear_x = average_rps_x * wheel_circumference_; // m/s
        
            //convert average revolutions per minute in y axis to revolutions per second
            //average_rps_y = ((float)(-rpm1 + rpm2) / 2) / 60; // RPM
        
            vel.linear_y = 0;
        
            //convert average revolutions per minute to revolutions per second
            average_rps_a = ((float)(rpm1 + rpm2) / 2) / 60;
            vel.angular_z =  -(average_rps_a * wheel_circumference_) / ((wheels_x_distance_ / 2) + (wheels_y_distance_ / 2)); //  rad/s
        
        }
        
        
        int16_t RikibotFocDriver::map(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max)
        {
          return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
        }
        
        
        int main(int argc, char * argv[])
        {
          rclcpp::init(argc, argv);
          auto driver = std::make_shared<RikibotFocDriver>();
          rclcpp::spin(driver);
          rclcpp::shutdown();
          return 0;
        }
        
        
        小鱼小 1 条回复 最后回复 回复 引用 0
        • 小鱼小
          小鱼 技术大佬 @毛哥成山轮胎机油保养
          最后由 编辑

          @毛哥成山轮胎机油保养 在 求助--底盘控制板代码谁能帮忙改一下? 中说:

          int16_t rpmR; //右轮转速
          int16_t rpmL; //左轮转速

          如果有左右轮转速信息就可以积分得出里程计数据,可以参考代码

          • https://blog.csdn.net/su_fei_ma_su/article/details/115298978
          • https://github.com/fishros/fishbot/blob/feature/base_control/src/fishbot_controller/fishbot_controller/driver/base/base_control.py

          新书配套视频:https://www.bilibili.com/video/BV1GW42197Ck/

          1 条回复 最后回复 回复 引用 0
          • 第一个帖子
            最后一个帖子
          皖ICP备16016415号-7
          Powered by NodeBB | 鱼香ROS