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

毛哥成山轮胎机油保养

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

毛哥成山轮胎机油保养

控制板实物图

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

底盘控制板代码目录结构

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;
}

小鱼

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

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

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