本文主要介绍怎么编写代码实现在上层修改底层参数的相关知识，主要讲解使用两种通信方式来实现
1、rosserial方法，rosserial是官方对arduino提供的一种简单的通信协议，本人对这个库进行了一些改动，使其可以在STM32上进行使用，通过使用这个协议可以在STM32或者Arduino发布和订阅节点数据，详细的移植和使用参考之前的两篇博客：ros下使用rosserial和STM32F1/STM32F4系列进行通信（MDK5工程）：https://blog.csdn.net/qq_36349536/article/details/82773064 STM32(MDK)中如何使用ROS自定义的msgs：https://blog.csdn.net/qq_36349536/article/details/84146531
在移植好了通信之后下面进行讲解怎么通过上层对底层参数进行修改，
主要涉及的知识点是参数服务Parameter Server可以参考官网的相关教程和示例http://wiki.ros.org/roscpp/Overview/Parameter%20Server
这里我以PID参数为示例给大家进行讲解，在底层启动的时候我们先等待上层把PID参数传递过来，然后再对各模块进行初始化，获取PID参数的代码如图：
在上层中我们主要是在一个.yaml文件中队这些参数进行设置如下图：
注意名称是要相互对应的。然后在launch文件中去加载这个.yaml的参数即可，具体写法如下图：
然后即可完成上层对底层参数的修改，例如我们在动态PID调参时，把参数调节好了，又不想去改底层的代码就可以这样做，这样在动态PID调参好后我们把对应的值写到配置文件xxx.yaml中再重启地盘即可完成参数的配置，如果大家想把参数保存在STM32中还可以学习一下STM32的怎么把数据写入flash相关的知识。
2、常规串口通信，常规串口通信就是需要自己定义数据格式，然后在上层中获取到参数服务器的值，再通过串口把对应的参数传给底层，底层接收后再对各模块进行初始化，这种方式比较通用，但是相对复杂。
底层串口通信代码如下：
#include "usart1.h"
#include <stdarg.h>
#include "string.h"
u8 usart1_rx_irq_updata_user_reset_status = 0;
u8 usart1_tx_irq_updata_user_reset_status = 0;
u8 usart1_tx_buf[USART1_TX_BUF_LENGTH];
//u8 usart1_rx_buf[USART1_RX_BUF_LENGTH];
unsigned char Rcount = 0;
/*
* 函数名：USART1_Init
* 描述 ：串口1初始化函数
* 输入 ：波特率 9600 119200 38400 57600 115200
* 输出 ：无
*/
void USART1_Init(u32 baudrate)
{
//GPIO端口设置
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE); //使能GPIOD时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);//使能USART1时钟，注意串口1的时钟是挂在APB2上的
//USART1端口配置
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_9; //GPIOA10与GPIOA9
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; //复用功能
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //速度50MHz
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽复用输出
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉
GPIO_Init(GPIOA,&GPIO_InitStructure);
//串口1对应引脚复用映射
GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_USART1);
GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1);
//USART1 初始化设置
USART_InitStructure.USART_BaudRate = baudrate; //波特率设置
USART_InitStructure.USART_WordLength = USART_WordLength_8b; //字长为8位数据格式
USART_InitStructure.USART_StopBits = USART_StopBits_1; //一个停止位
USART_InitStructure.USART_Parity = USART_Parity_No; //无奇偶校验位
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //无硬件数据流控制
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; //收发模式
USART_Init(USART1, &USART_InitStructure);
USART_Cmd(USART1, ENABLE);
USART_ITConfig(USART1, USART_IT_IDLE, ENABLE); //开启USART空闲中断
NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn; //串口1中断通道
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; //抢占优先级3
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; //子优先级3
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道使能
NVIC_Init(&NVIC_InitStructure); //根据指定的参数初始化VIC寄存器
USART1_DMA_Init();
}
void USART1_DMA_Init(void)
{
DMA_InitTypeDef DMA_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2,ENABLE); //DMA2时钟使能
USART_DMACmd(USART1,USART_DMAReq_Tx,ENABLE); //使能串口1的DMA发送
USART_DMACmd(USART1,USART_DMAReq_Rx,ENABLE); //使能串口1的DMA接收
//****************************配置USART1发送 TX
DMA_DeInit(DMA2_Stream7); //DMA数据流
while (DMA_GetCmdStatus(DMA2_Stream7) != DISABLE); //等待DMA可配置
// 配置 DMA Stream
DMA_InitStructure.DMA_Channel = DMA_Channel_4; //通道选择
DMA_InitStructure.DMA_PeripheralBaseAddr = (u32)&USART1->DR; //DMA外设地址
DMA_InitStructure.DMA_Memory0BaseAddr = (u32)usart1_tx_buf; //DMA 存储器0地址
DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; //存储器到外设模式
DMA_InitStructure.DMA_BufferSize = USART1_TX_BUF_LENGTH; //数据传输量
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; //外设非增量模式
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; //存储器增量模式
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; //外设数据长度:8位
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; //存储器数据长度:8位
DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; //使用普通模式
DMA_InitStructure.DMA_Priority = DMA_Priority_Medium; //中等优先级
DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;
DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; //存储器突发单次传输
DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; //外设突发单次传输
DMA_Init(DMA2_Stream7, &DMA_InitStructure); //初始化DMA Stream
//DMA NVIC
NVIC_InitStructure.NVIC_IRQChannel = DMA2_Stream7_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
DMA_ITConfig(DMA2_Stream7,DMA_IT_TC,ENABLE);
//****************************配置UART1接收 //RX
DMA_DeInit(DMA2_Stream5);
while (DMA_GetCmdStatus(DMA2_Stream5) != DISABLE); //等待DMA可配置
DMA_InitStructure.DMA_Channel = DMA_Channel_4; //通道选择
DMA_InitStructure.DMA_PeripheralBaseAddr = (u32)&USART1->DR; //DMA外设地址
DMA_InitStructure.DMA_Memory0BaseAddr = (u32)usart1_rx_buf; //DMA 存储器0地址
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory ; //外设到存储器模式
DMA_InitStructure.DMA_BufferSize = USART1_RX_BUF_LENGTH; //数据传输量
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; //外设非增量模式
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; //存储器增量模式
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; //外设数据长度:8位
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; //存储器数据长度:8位
DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; //使用普通模式
DMA_InitStructure.DMA_Priority = DMA_Priority_High; //中等优先级
DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;
DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; //存储器突发单次传输
DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; //外设突发单次传输
DMA_Init(DMA2_Stream5, &DMA_InitStructure); //初始化DMA Stream
DMA_Cmd(DMA2_Stream5, ENABLE); //开启DMA传输
usart1_rx_irq_updata_user_reset_status = 0;
usart1_tx_irq_updata_user_reset_status = 0;
}
//发送TX
void DMA2_Stream7_IRQHandler(void)
{
if(DMA_GetITStatus(DMA2_Stream7,DMA_IT_TCIF7) != RESET)
{
DMA_Cmd(DMA2_Stream7,DISABLE); //DISABLE DMA
DMA_ClearFlag(DMA2_Stream7,DMA_FLAG_TCIF7);
usart1_tx_irq_updata_user_reset_status = 0;
}
}
/*
* Function Name:USART1_DataFrame_Send
* Description :
* Input :send_buf, send_buf length
* Output :None
*/
void USART1_DataFrame_Send(unsigned char *send_buf,int length)
{
if(usart1_tx_irq_updata_user_reset_status == 0)
{
memcpy(&usart1_tx_buf,send_buf,length);
DMA_SetCurrDataCounter(DMA2_Stream7,length);//设置传输数据长度
DMA_Cmd(DMA2_Stream7,ENABLE); //启动DMA
USART_DMACmd(USART1, USART_DMAReq_Tx, ENABLE);
usart1_tx_irq_updata_user_reset_status = 1;
}
}
//接收RX
void USART1_IRQHandler(void)
{
if(USART_GetITStatus(USART1,USART_IT_IDLE) != RESET)
{
DMA_Cmd(DMA1_Stream5, DISABLE);
USART1->SR;
USART1->DR;
DMA_ClearFlag(DMA2_Stream5,DMA_FLAG_TCIF5 | DMA_FLAG_FEIF5 | DMA_FLAG_DMEIF5 |
DMA_FLAG_TEIF5 | DMA_FLAG_HTIF5);//清除DMA2_Steam7传输完成标志
DMA_SetCurrDataCounter(DMA2_Stream5, USART1_RX_BUF_LENGTH);
DMA_Cmd(DMA2_Stream5, ENABLE);
usart1_rx_irq_updata_user_reset_status = 1;
}
}
//串口2,printf 函数
//确保一次发送数据不超过USART1_MAX_SEND_LEN字节
void u1_printf(char* fmt,...)
{
u16 i;
va_list ap;
va_start(ap,fmt);
vsprintf((char*)usart1_tx_buf,fmt,ap);
va_end(ap);
i=strlen((const char*)usart1_tx_buf); //此次发送数据的长度
USART1_DataFrame_Send(usart1_tx_buf,i);
}
#ifndef __USART1_H
#define __USART1_H
#include "stdio.h"
#include "pconfig.h"
#include "usart2.h"
#include "stm32f4xx.h"
#define PROTOCOL_HEADER 0XFEFEFEFE
#define PROTOCOL_END 0XEE
#define PROTOCL_DATA_SIZE 289
#define USART1_RX_BUF_LENGTH 289 //定义最大接收字节数 200
#define USART1_TX_BUF_LENGTH 289 //发送最大字节数
extern u8 usart1_rx_buf[];
//extern u8 usart1_tx_buf[];
//每个特定平台上的编译器都有自己的默认“对齐系数”(也叫对齐模数)。程序员可以通过预编译命令#pragma pack(n)，n=1,2,4,8,16来改变这一系数
//其中的n就是你要指定的“对齐系数”。
//规则：
// 1、数据成员对齐规则：结构(struct)(或联合(union))的数据成员，第一个数据成员放在offset为0的地方，
// 以后每个数据成员的对齐按照#pragma pack指定的数值和这个数据成员自身长度中，比较小的那个进行。
// 2、结构(或联合)的整体对齐规则：在数据成员完成各自对齐之后，结构(或联合)本身也要进行对齐，
// 对齐将按照#pragma pack指定的数值和结构(或联合)最大数据成员长度中，比较小的那个进行。
#pragma pack(1)
typedef struct __IMU_Data_
{
float X_data;
float Y_data;
float Z_data;
}IMU_Data;
//32位编译器：
//char ：1个字节
//char*（即指针变量）: 4个字节（32位的寻址空间是2^32, 即32个bit，也就是4个字节。同理64位编译器）
//short int : 2个字节
// int： 4个字节
//unsigned int : 4个字节
//float: 4个字节
//double: 8个字节
//long: 4个字节
//long long: 8个字节
//unsigned long: 4个字节
typedef union _Upload_Data_
{
unsigned char buffer[PROTOCL_DATA_SIZE];
struct _Sensor_Str_
{
unsigned int Header;
int InitParameter; //参数初始化标志位
int Base_Type; //车子类型
int Motor_Drive_Type; //电机驱动类型
int Pwm_Max; //PWM最大值
int Pwm_Min; //PWM最大值
int Max_Rpm; //电机最大转速
int Pwm_Bits; //PWM分辨率
int Motor_Reduction_Ratio; //电机减速比
int Encoder_Line_Number; //编码器分辨率
int Counts_Per_Rev; //编码器转动一周的计数值
int DebunMotor; //电机调试标志位
float Kp_; //PID的p参数
float Ki_; //PID的i参数
int Kd_; //PID的i参数
int PID_Debug_Enable; //PID动态调参使能标准位
float Wheel_Diameter; //轮子直径
float Lr_Wheels_Distance; //机器人左右轮子距离
float Fr_Wheels_Distance; //机器人前后轮子距离
int Direction_Motor1_Rotation; //电机转动方向标志位
int Direction_Motor2_Rotation; //电机转动方向标志位
int Direction_Motor3_Rotation; //电机转动方向标志位
int Direction_Motor4_Rotation; //电机转动方向标志位
int Direction_Encoder1_Value; //编码器计数方向标志位
int Direction_Encoder2_Value; //编码器计数方向标志位
int Direction_Encoder3_Value; //编码器计数方向标志位
int Direction_Encoder4_Value; //编码器计数方向标志位
long long Encoder1_Value; //编码器计数值
long long Encoder2_Value; //编码器计数值
long long Encoder3_Value; //编码器计数值
long long Encoder4_Value; //编码器计数值
float X_Speed; //右手坐标系的正解速度信息X
float Y_Speed; //右手坐标系的正解速度信息Y
float Z_Speed; //右手坐标系的正解速度信息Z
float Voltage; //电池当前电压
float Electricity; //电池输出电流
int Employ_Electricity; //已经使用电量
int Dump_Energy; //剩余电量
float Battery_Voltage_Used; //使用的电池电压值12或者24
int IMU_Type; //使用的IMU类型
IMU_Data Link_Accelerometer; //IMU的三轴加速度原始数据
IMU_Data Link_Gyroscope; //IMU的三轴角速度原始数据
IMU_Data Link_Magnetometer; //IMU的三轴磁力计原始数据
int Ultrasonic1_Enable; //超声波1使能标志位
int Ultrasonic2_Enable; //超声波2使能标志位
int Ultrasonic3_Enable; //超声波3使能标志位
float Ultrasonic1_Data; //超声波1数据
float Ultrasonic2_Data; //超声波2数据
float Ultrasonic3_Data; //超声波3数据
int Servo1_Enable; //舵机使能标志位
int Servo2_Enable; //舵机使能标志位
int Servo3_Enable; //舵机使能标志位
int Servo4_Enable; //舵机使能标志位
int Servo1_Data; //舵机转动角度值
int Servo2_Data; //舵机转动角度值
int Servo3_Data; //舵机转动角度值
int Servo4_Data; //舵机转动角度值
int Max_Steering_Angle; //舵机最大转动角度值
int Initial_Turning_Angle; //舵机初始化角度值
int Esc_Median_Value; //电调中值
int A2_Encoder_Enable; //A2车型编码器使能标志位
int Emergency_Stop_Enable; //急停按钮使能标志位
unsigned char End_flag; //包尾结束标志
}Sensor_Str;//55*4+12*3+8*4+1 =197+18=289
}Upload_Data;
#pragma pack(4)
extern Upload_Data Send_Data, Recive_Data;
extern float send_data[4];
void USART1_Init(u32 baudrate);
void USART1_DMA_Init(void);
void USART1_DataFrame_Send (unsigned char *send_buf,int length);
void u1_printf(char* fmt,...);
bool PC_SendTo_Base(void);
#endif
上层代码如下：
#include "starrobot_robot.h"
starrobot_robot_object::starrobot_robot_object():
baud_data(115200),lr_wheels_distance_(1.0),fr_wheels_distance_(1.0),wheel_diameter_(1.0),
max_rpm_(366),pwm_bits_(8),pwm_max_(256),pwm_min_(-256),
motor_reduction_ratio_(30),encoder_line_number_(13),counts_per_rev_(0),
Direction_motor1_rotation_(1),Direction_motor2_rotation_(1),
Direction_motor3_rotation_(1),Direction_motor4_rotation_(1),
Direction_encoder1_value_(1),Direction_encoder2_value_(1),
Direction_encoder3_value_(1),Direction_encoder4_value_(1),
ultrasonic1_Enable_(0),ultrasonic2_Enable_(0),ultrasonic3_Enable_(0),
servo1_Enable_(0),servo2_Enable_(0),servo3_Enable_(0),servo4_Enable_(0),
initial_turning_angle_(0),max_steering_angle_(30),
esc_median_value_(1500),a2_encoder_(0),emergency_stop_(0),
k_p_(0.1),k_i_(0.1),k_d_(0.1),debug_message_(1),debug_base_(0),
base_data_num(1),imu_data_num(1),motor_data_num(1),wheel_num(2),
cmd_vel_sub_name_("cmd_vel"),swerve_sub_name_("initial_angle"),
pid_sub_name_("pid"),servo_sub_name_("servo"),raw_vel_pub_name_("raw_vel"),
raw_imu_pub_name_("raw_imu"),battery_pub_name_("battery"),sonar_pub_name_("sonar")
{
memset(&Reciver_Str, 0, sizeof(Reciver_Str));
memset(&Send_Str, 0, sizeof(Send_Str));
this->Send_Str.Sensor_Str.Header = PROTOCOL_HEADER;
this->Send_Str.Sensor_Str.End_flag = PROTOCOL_END;
// Get Luncher file define value
ros::NodeHandle nh_private("~");
nh_private.param<std::string>("port", this->usart_port, "/dev/starrobot_base");
nh_private.param<int>("baud", this->baud_data, 115200);
nh_private.getParam("cmd_vel_sub_name",cmd_vel_sub_name_);
nh_private.getParam("swerve_sub_name",swerve_sub_name_);
nh_private.getParam("pid_sub_name",pid_sub_name_);
nh_private.getParam("servo_sub_name",servo_sub_name_);
nh_private.getParam("raw_vel_pub_name",raw_vel_pub_name_);
nh_private.getParam("raw_imu_pub",raw_imu_pub_name_);
nh_private.getParam("battery_pub",battery_pub_name_);
nh_private.getParam("sonar_pub",sonar_pub_name_);
raw_vel_pub = nh_.advertise<starrobot_msgs::Velocities>(raw_vel_pub_name_, 10);
raw_imu_pub = nh_.advertise<starrobot_msgs::Imu>(raw_imu_pub_name_, 10);
battery_pub = nh_.advertise<starrobot_msgs::Battery>(battery_pub_name_, 10);
sonar_pub = nh_.advertise<starrobot_msgs::Sonar>(sonar_pub_name_, 10);
cmd_vel_sub = nh_.subscribe(cmd_vel_sub_name_, 10, &starrobot_robot_object::cmd_velCallback, this);
swerve_sub = nh_.subscribe(swerve_sub_name_, 10, &starrobot_robot_object::swerveCallback, this);
pid_sub = nh_.subscribe(pid_sub_name_, 10, &starrobot_robot_object::pid_velCallback, this);
servo_sub = nh_.subscribe(servo_sub_name_, 10, &starrobot_robot_object::servo_velCallback, this);
/**open seril device**/
try{
Robot_Serial.setPort(this->usart_port);
Robot_Serial.setBaudrate(this->baud_data);
serial::Timeout to = serial::Timeout::simpleTimeout(2000);
Robot_Serial.setTimeout(to);
Robot_Serial.open();
}
catch (serial::IOException& e){
ROS_ERROR_STREAM("[starrobot] Unable to open serial port, please check device or permission");
}
if(Robot_Serial.isOpen())
{
ROS_INFO_STREAM("[starrobot] Successful opening of the serial port, data transmission started");
}
else
{
ROS_ERROR_STREAM("[starrobot] Unable to open serial port, please check device or permission");
}
}
starrobot_robot_object::~starrobot_robot_object()
{
Robot_Serial.close();
}
/***
@ Description -> base init function
@ Param -> NULL
@ Author -> ChenZhouFu
@ Date -> 2020-4-1
@ Function -> starrobot_robot_object::base_InitParam()
***/
void starrobot_robot_object::base_InitParam()
{
ros::NodeHandle private_nh("~");
private_nh.getParam("/starrobot_base",starrobot_base_);
private_nh.getParam("/imu_type",imu_type_);
private_nh.getParam("/starrobot_motor_drive",starrobot_motor_drive_);
private_nh.getParam("/debug_message",debug_message_);
private_nh.getParam("/debug_base",debug_base_);
private_nh.getParam("/motor_in/pwm_max",pwm_max_);
private_nh.getParam("/motor_in/pwm_min",pwm_min_);
private_nh.getParam("/max_rpm",max_rpm_);
private_nh.getParam("/pwm_bits",pwm_bits_);
private_nh.getParam("/motor_reduction_ratio",motor_reduction_ratio_);
private_nh.getParam("/encoder_line_number",encoder_line_number_);
private_nh.getParam("/counts_per_rev",counts_per_rev_);
private_nh.getParam("/pid/k_p",k_p_);
private_nh.getParam("/pid/k_i",k_i_);
private_nh.getParam("/pid/k_d",k_d_);
private_nh.getParam("/emergency_stop",emergency_stop_);
private_nh.getParam("/wheel_diameter",wheel_diameter_);
private_nh.getParam("/lr_wheels_distance",lr_wheels_distance_);
private_nh.getParam("/fr_wheels_distance",fr_wheels_distance_);
private_nh.getParam("/Direction_motor1_rotation",Direction_motor1_rotation_);
private_nh.getParam("/Direction_motor2_rotation",Direction_motor2_rotation_);
private_nh.getParam("/Direction_motor3_rotation",Direction_motor3_rotation_);
private_nh.getParam("/Direction_motor4_rotation",Direction_motor4_rotation_);
private_nh.getParam("/Direction_encoder1_value",Direction_encoder1_value_);
private_nh.getParam("/Direction_encoder2_value",Direction_encoder2_value_);
private_nh.getParam("/Direction_encoder3_value",Direction_encoder3_value_);
private_nh.getParam("/Direction_encoder4_value",Direction_encoder4_value_);
private_nh.getParam("/ultrasonic/ultrasonic_1",ultrasonic1_Enable_);
private_nh.getParam("/ultrasonic/ultrasonic_2",ultrasonic2_Enable_);
private_nh.getParam("/ultrasonic/ultrasonic_3",ultrasonic3_Enable_);
private_nh.getParam("/servo/servo_1",servo1_Enable_);
private_nh.getParam("/servo/servo_2",servo2_Enable_);
private_nh.getParam("/servo/servo_3",servo3_Enable_);
private_nh.getParam("/servo/servo_4",servo4_Enable_);
private_nh.getParam("/initial_turning_angle",initial_turning_angle_);
private_nh.getParam("/max_steering_angle",max_steering_angle_);
private_nh.getParam("/esc_median_value",esc_median_value_);
private_nh.getParam("/a2_encoder",a2_encoder_);
private_nh.getParam("/emergency_stop",emergency_stop_);
Send_Str.Sensor_Str.Pwm_Max = pwm_max_;
Send_Str.Sensor_Str.Pwm_Min = pwm_min_;
Send_Str.Sensor_Str.Max_Rpm = max_rpm_;
Send_Str.Sensor_Str.Pwm_Bits = pwm_bits_;
Send_Str.Sensor_Str.Motor_Reduction_Ratio = motor_reduction_ratio_;
Send_Str.Sensor_Str.Encoder_Line_Number = encoder_line_number_;
Send_Str.Sensor_Str.Counts_Per_Rev = counts_per_rev_;
if(starrobot_base_ == "a2")
{
Send_Str.Sensor_Str.Base_Type = 1;
wheel_num = 2;
}
if(starrobot_base_ == "a2")
{
Send_Str.Sensor_Str.Base_Type = 2;
wheel_num = 1;
}
if(starrobot_base_ == "d2")
{
Send_Str.Sensor_Str.Base_Type = 3;
wheel_num = 2;
}
if(starrobot_base_ == "d4")
{
Send_Str.Sensor_Str.Base_Type = 4;
wheel_num = 4;
}
if(starrobot_base_ == "o3")
{
Send_Str.Sensor_Str.Base_Type = 5;
wheel_num = 3;
}
if(starrobot_base_ == "o4")
{
Send_Str.Sensor_Str.Base_Type = 6;
wheel_num = 4;
}
if(starrobot_base_ == "m4")
{
Send_Str.Sensor_Str.Base_Type = 7;
wheel_num = 4;
};
if(starrobot_base_ == "t2")
{
Send_Str.Sensor_Str.Base_Type = 8;
wheel_num = 2;
}
if(starrobot_base_ == "t4")
{
Send_Str.Sensor_Str.Base_Type = 9;
wheel_num = 4;
}
if(imu_type_ == "GY85")
Send_Str.Sensor_Str.IMU_Type = 1;
if(imu_type_ == "MPU6050")
Send_Str.Sensor_Str.IMU_Type = 2;
if(imu_type_ == "MPU9250_N")
Send_Str.Sensor_Str.IMU_Type = 3;
if(imu_type_ == "MPU9250_W")
Send_Str.Sensor_Str.IMU_Type = 4;
if(imu_type_ == "MPU6500_N")
Send_Str.Sensor_Str.IMU_Type = 5;
if(imu_type_ == "MPU6500_W")
Send_Str.Sensor_Str.IMU_Type = 6;
if(starrobot_motor_drive_ == "A4950")
Send_Str.Sensor_Str.Motor_Drive_Type = 1;
if(starrobot_motor_drive_ == "BTN79xx")
Send_Str.Sensor_Str.Motor_Drive_Type = 2;
if(starrobot_motor_drive_ == "ESC")
Send_Str.Sensor_Str.Motor_Drive_Type = 3;
if(starrobot_motor_drive_ == "ESC_ENCODER")
Send_Str.Sensor_Str.Motor_Drive_Type = 4;
Send_Str.Sensor_Str.Kp_ = k_p_;
Send_Str.Sensor_Str.Ki_ = k_i_;
Send_Str.Sensor_Str.Kd_ = k_d_;
Send_Str.Sensor_Str.PID_Debug_Enable = 0;
Send_Str.Sensor_Str.Wheel_Diameter = wheel_diameter_;
Send_Str.Sensor_Str.Lr_Wheels_Distance = lr_wheels_distance_;
Send_Str.Sensor_Str.Fr_Wheels_Distance = fr_wheels_distance_;
Send_Str.Sensor_Str.Direction_Motor1_Rotation = Direction_motor1_rotation_;
Send_Str.Sensor_Str.Direction_Motor2_Rotation = Direction_motor2_rotation_;
Send_Str.Sensor_Str.Direction_Motor3_Rotation = Direction_motor3_rotation_;
Send_Str.Sensor_Str.Direction_Motor4_Rotation = Direction_motor4_rotation_;
Send_Str.Sensor_Str.Direction_Encoder1_Value = Direction_encoder1_value_;
Send_Str.Sensor_Str.Direction_Encoder2_Value = Direction_encoder2_value_;
Send_Str.Sensor_Str.Direction_Encoder3_Value = Direction_encoder3_value_;
Send_Str.Sensor_Str.Direction_Encoder4_Value = Direction_encoder4_value_;
Send_Str.Sensor_Str.Ultrasonic1_Enable = ultrasonic1_Enable_;
Send_Str.Sensor_Str.Ultrasonic2_Enable = ultrasonic2_Enable_;
Send_Str.Sensor_Str.Ultrasonic3_Enable = ultrasonic3_Enable_;
Send_Str.Sensor_Str.Servo1_Enable = servo1_Enable_;
Send_Str.Sensor_Str.Servo2_Enable = servo2_Enable_;
Send_Str.Sensor_Str.Servo3_Enable = servo3_Enable_;
Send_Str.Sensor_Str.Servo4_Enable = servo4_Enable_;
Send_Str.Sensor_Str.Max_Steering_Angle = initial_turning_angle_;
Send_Str.Sensor_Str.Initial_Turning_Angle = max_steering_angle_;
Send_Str.Sensor_Str.Esc_Median_Value = esc_median_value_;
Send_Str.Sensor_Str.A2_Encoder_Enable = a2_encoder_;
Send_Str.Sensor_Str.Emergency_Stop_Enable = emergency_stop_;
Send_Str.Sensor_Str.InitParameter = 1;
Send_Str.Sensor_Str.DebunMotor = debug_base_;
ROS_INFO("starrobot_base:%s",starrobot_base_.c_str());
ROS_INFO("imu_type :%s",imu_type_.c_str());
ROS_INFO("motor_drive :%s",starrobot_motor_drive_.c_str());
ROS_INFO("debug_message:%d debug_base:%d",debug_message_,debug_base_);
ROS_INFO("pwm_max:%d pwm_min:%d max_rpm:%d",pwm_max_,pwm_min_,max_rpm_);
ROS_INFO("pwm_bits:%d motor_reduction_ratio:%d",pwm_bits_,motor_reduction_ratio_);
ROS_INFO("encoder_line_number:%d counts_per_rev:%d",encoder_line_number_,counts_per_rev_);
ROS_INFO("k_p:%.3f k_i:%.3f k_d:%.3f",k_p_,k_i_,k_d_);
ROS_INFO("wheel_diameter:%.3f",wheel_diameter_);
ROS_INFO("lr_wheels_distance:%.3f",lr_wheels_distance_);
ROS_INFO("fr_wheels_distance:%.3f",fr_wheels_distance_);
ROS_INFO("Direction_motor1:%d Direction_motor2:%d",
Direction_motor1_rotation_,Direction_motor2_rotation_);
ROS_INFO("Direction_motor3:%d Direction_motor4:%d",
Direction_motor3_rotation_,Direction_motor4_rotation_);
ROS_INFO("Direction_encoder1:%d Direction_encoder2:%d",
Direction_encoder1_value_,Direction_encoder2_value_);
ROS_INFO("Direction_encoder3:%d Direction_encoder4:%d",
Direction_encoder3_value_,Direction_encoder4_value_);
ROS_INFO("ultrasonic1_Enable:%d",ultrasonic1_Enable_);
ROS_INFO("ultrasonic2_Enable:%d",ultrasonic1_Enable_);
ROS_INFO("ultrasonic3_Enable:%d",ultrasonic1_Enable_);
ROS_INFO("servo1_Enable:%d",servo1_Enable_);
ROS_INFO("servo2_Enable:%d",servo2_Enable_);
ROS_INFO("servo3_Enable:%d",servo3_Enable_);
ROS_INFO("servo4_Enable:%d",servo4_Enable_);
ROS_INFO("initial_turning_angle:%d",initial_turning_angle_);
ROS_INFO("max_steering_angle:%d",max_steering_angle_);
ROS_INFO("esc_median_value:%d",esc_median_value_);
ROS_INFO("a2_encoder:%d",a2_encoder_);
ROS_INFO("emergency_stop:%d",emergency_stop_);
Robot_Serial.write(Send_Str.buffer, sizeof(Send_Str.buffer));
}
/***
@ Description -> cmd_vel Callback function
@ Param -> const geometry_msgs::Twist &twist_aux
@ Author -> ChenZhouFu
@ Date -> 2020-4-1
@ Function -> starrobot_robot_object::cmd_velCallback(const geometry_msgs::Twist &twist_aux)
***/
void starrobot_robot_object::cmd_velCallback(const geometry_msgs::Twist &twist_aux)
{
// process callback function msgs
Send_Str.Sensor_Str.X_Speed = twist_aux.linear.x;
Send_Str.Sensor_Str.Y_Speed = twist_aux.linear.y;
Send_Str.Sensor_Str.Z_Speed = twist_aux.angular.z;
Robot_Serial.write(Send_Str.buffer, sizeof(Send_Str.buffer));
}
/***
@ Description -> swerve Callback function
@ Param -> const geometry_msgs::Twist &twist_aux
@ Author -> ChenZhouFu
@ Date -> 2020-4-1
@ Function -> starrobot_robot_object::swerveCallback(const std_msgs::Int16& swerve_servo)
***/
void starrobot_robot_object::swerveCallback(const std_msgs::Int16& swerve_servo)
{
// process callback function msgs
Send_Str.Sensor_Str.Initial_Turning_Angle = swerve_servo.data;
Robot_Serial.write(Send_Str.buffer, sizeof(Send_Str.buffer));
}
/***
@ Description -> pid Callback function
@ Param -> const const starrobot_msgs::PID& pid
@ Author -> ChenZhouFu
@ Date -> 2020-4-1
@ Function -> starrobot_robot_object::pid_velCallback(const starrobot_msgs::PID& pid)
***/
void starrobot_robot_object::pid_velCallback(const starrobot_msgs::PID& pid)
{
// process callback function msgs
Send_Str.Sensor_Str.Kp_ = pid.p;
Send_Str.Sensor_Str.Ki_ = pid.i;
Send_Str.Sensor_Str.Kd_ = pid.d;
Send_Str.Sensor_Str.PID_Debug_Enable = 1;
Robot_Serial.write(Send_Str.buffer, sizeof(Send_Str.buffer));
}
/***
@ Description -> servo Callback function
@ Param -> const const starrobot_msgs::Servo& servo_data
@ Author -> ChenZhouFu
@ Date -> 2020-4-1
@ Function -> starrobot_robot_object::servo_velCallback(const starrobot_msgs::Servo& servo_data)
***/
void starrobot_robot_object::servo_velCallback(const starrobot_msgs::Servo& servo_data)
{
// process callback function msgs
Send_Str.Sensor_Str.Servo1_Data = servo_data.servo1;
Send_Str.Sensor_Str.Servo2_Data = servo_data.servo2;
Send_Str.Sensor_Str.Servo3_Data = servo_data.servo3;
Send_Str.Sensor_Str.Servo4_Data = servo_data.servo4;
Robot_Serial.write(Send_Str.buffer, sizeof(Send_Str.buffer));
}
/***
@ Description -> send and get stm32 board data
@ Param -> null
@ Author -> ChenZhouFu
@ Date -> 2019-03-10
@ Function -> bool starrobot_robot_object::ReadFormUart()
@ return -> status
***/
bool starrobot_robot_object::ReadFormUart()
{
unsigned char CheckSumBuffer[1];
Robot_Serial.read(Reciver_Str.buffer,sizeof(Reciver_Str.buffer));
if (Reciver_Str.Sensor_Str.Header == PROTOCOL_HEADER)
{
if (Reciver_Str.Sensor_Str.End_flag == PROTOCOL_END)
{
PublisherRawImu();
PublisherRawVel();
PublisherBattery();
PublisherSonar();
if(debug_base_)
{
DebugEncoderDataShow();
}
return true;
}
}
//Robot_Serial.read(CheckSumBuffer,sizeof(CheckSumBuffer));
return false;
}
/***
@ Description -> Publisher RawVel
@ Param -> null
@ Author -> ChenZhouFu
@ Date -> 2019-03-10
@ Function -> starrobot_robot_object::PublisherRawVel(void)
@ return -> NULL
***/
void starrobot_robot_object::PublisherRawVel(void)
{
raw_vel_msg.linear_y = Reciver_Str.Sensor_Str.X_Speed;
raw_vel_msg.linear_x = Reciver_Str.Sensor_Str.Y_Speed;
raw_vel_msg.angular_z = Reciver_Str.Sensor_Str.Z_Speed;
raw_vel_pub.publish(raw_vel_msg);
}
/***
@ Description -> Publisher RawImu()
@ Param -> null
@ Author -> ChenZhouFu
@ Date -> 2020-04-1
@ Function -> starrobot_robot_object::PublisherRawImu()
@ return -> null
***/
void starrobot_robot_object::PublisherRawImu(void)
{
raw_imu_msg.linear_acceleration.x = Reciver_Str.Sensor_Str.Link_Accelerometer.X_data;
raw_imu_msg.linear_acceleration.y = Reciver_Str.Sensor_Str.Link_Accelerometer.Y_data;
raw_imu_msg.linear_acceleration.z = Reciver_Str.Sensor_Str.Link_Accelerometer.Z_data;
raw_imu_msg.angular_velocity.x = Reciver_Str.Sensor_Str.Link_Gyroscope.X_data;
raw_imu_msg.angular_velocity.y = Reciver_Str.Sensor_Str.Link_Gyroscope.Y_data;
raw_imu_msg.angular_velocity.z = Reciver_Str.Sensor_Str.Link_Gyroscope.Z_data;
raw_imu_msg.magnetic_field.x = Reciver_Str.Sensor_Str.Link_Magnetometer.X_data;
raw_imu_msg.magnetic_field.y = Reciver_Str.Sensor_Str.Link_Magnetometer.Y_data;
raw_imu_msg.magnetic_field.z = Reciver_Str.Sensor_Str.Link_Magnetometer.Z_data;
raw_imu_pub.publish(raw_imu_msg);
}
/***
@ Description -> Publisher Battery
@ Param -> null
@ Author -> ChenZhouFu
@ Date -> 2019-04-1
@ Function -> starrobot_robot_object::PublisherBattery()
@ return -> NULL
***/
void starrobot_robot_object::PublisherBattery(void)
{
raw_bat_msg.voltage = Reciver_Str.Sensor_Str.Voltage;
raw_bat_msg.electricity = Reciver_Str.Sensor_Str.Electricity;
battery_pub.publish(raw_bat_msg);
}
/***
@ Description -> Publisher Sonar
@ Param -> null
@ Author -> ChenZhouFu
@ Date -> 2020-4-1
@ Function -> starrobot_robot_object::PublisherSonar()
@ return -> NULL
***/
void starrobot_robot_object::PublisherSonar(void)
{
raw_sonar_msg.sonar1 = Reciver_Str.Sensor_Str.Ultrasonic1_Data;
raw_sonar_msg.sonar2 = Reciver_Str.Sensor_Str.Ultrasonic2_Data;
raw_sonar_msg.sonar3 = Reciver_Str.Sensor_Str.Ultrasonic3_Data;
sonar_pub.publish(raw_sonar_msg);
}
/***
@ Description -> Debug Encoder Data Show
@ Param -> null
@ Author -> ChenZhouFu
@ Date -> 2020-4-1
@ Function -> starrobot_robot_object::DebugEncoderDataShow()
@ return -> NULL
***/
void starrobot_robot_object::DebugEncoderDataShow(void)
{
switch(wheel_num)
{
case 1:
if(a2_encoder_)
ROS_INFO("A2 Encoder: %lld",Reciver_Str.Sensor_Str.Encoder1_Value);
break;
case 2:
ROS_INFO("Encoder Left Front 1 : %lld",Reciver_Str.Sensor_Str.Encoder1_Value);
ROS_INFO("Encoder Right Front 2 : %lld",Reciver_Str.Sensor_Str.Encoder2_Value);
break;
case 3:
ROS_INFO("Encoder Front 1 : %lld",Reciver_Str.Sensor_Str.Encoder1_Value);
ROS_INFO("Encoder Right 2 : %lld",Reciver_Str.Sensor_Str.Encoder2_Value);
ROS_INFO("Encoder Left 3 : %lld",Reciver_Str.Sensor_Str.Encoder3_Value);
break;
case 4:
ROS_INFO("Encoder Left Front 1 : %lld",Reciver_Str.Sensor_Str.Encoder1_Value);
ROS_INFO("Encoder Right Front 2 : %lld",Reciver_Str.Sensor_Str.Encoder2_Value);
ROS_INFO("Encoder Left Rear 3 : %lld",Reciver_Str.Sensor_Str.Encoder3_Value);
ROS_INFO("Encoder Right Rear 4 : %lld",Reciver_Str.Sensor_Str.Encoder4_Value);
break;
}
}
/***
@ Description -> Read And Write LoopP rocess
@ Param -> null
@ Author -> ChenZhouFu
@ Date -> 2020-4-1
@ Function -> starrobot_robot_object::DebugEncoderDataShow()
@ return -> NULL
***/
void starrobot_robot_object::ReadAndWriteLoopProcess(void)
{
while(ros::ok())
{
if(true == ReadFormUart())
{
PublisherRawVel();
PublisherRawImu();
PublisherBattery();
PublisherSonar();
}
}
}
#ifndef __HUANYU_ROBOT_H_
#define __HUANYU_ROBOT_H_
// sudo apt-get install ros-kinetic-serial ->kinetic
// sudo apt-get install ros-melodic-serial ->melodic
// rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.1, y: 0, z: 0}, angular: {x: 0, y: 0, z: -0.5}}'
// sudo usermod -aG dialout wsh
#include "ros/ros.h"
#include <iostream>
#include <string.h>
#include <string>
#include <iostream>
#include <math.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <serial/serial.h>
#include <fcntl.h>
#include <stdbool.h>
#include <tf/transform_broadcaster.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Twist.h>
#include <starrobot_msgs/Battery.h>
#include <starrobot_msgs/Crash.h>
#include <starrobot_msgs/Fall.h>
#include <starrobot_msgs/Imu.h>
#include <starrobot_msgs/Infrared.h>
#include <starrobot_msgs/Key.h>
#include <starrobot_msgs/PID.h>
#include <starrobot_msgs/PowerKey.h>
#include <starrobot_msgs/Servo.h>
#include <starrobot_msgs/Sonar.h>
#include <starrobot_msgs/Velocities.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Int16.h>
using namespace std;
#define PROTOCOL_HEADER 0XFEFEFEFE
#define PROTOCOL_END 0XEE
#define PROTOCL_DATA_SIZE 289
#define PI 3.1415926f
#define GYROSCOPE_RADIAN 0.001064f // gyro_x/(16.40*57.30)=gyro_x*0.001064 单位为弧度每秒
#define GYROSCOPE_DEGREE 16.40f // Accelerometer_Xdata/16.40=61度每秒
#define ACCELEROMETER 16384.0f // 1000/2048=0.49g。g为加速度的单位，重力加速度定义为1g, 等于9.8米每平方秒。
#define sampleFreq 20.0f // sample frequency in Hz
#define twoKpDef 1.0f // (2.0f * 0.5f) // 2 * proportional gain
#define twoKiDef 0.0f // (2.0f * 0.0f) // 2 * integral gain
#define OFFSET_COUNT 40
const double odom_pose_covariance[36] = {1e-3, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e3};
const double odom_pose_covariance2[36] = {1e-9, 0, 0, 0, 0, 0,
0, 1e-3, 1e-9, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-9};
const double odom_twist_covariance[36] = {1e-3, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e3};
const double odom_twist_covariance2[36] = {1e-9, 0, 0, 0, 0, 0,
0, 1e-3, 1e-9, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-9};
#pragma pack(1)
typedef struct __IMU_Data_
{
float X_data;
float Y_data;
float Z_data;
}IMU_Data;
//32位编译器：
//char ：1个字节
//char*（即指针变量）: 4个字节（32位的寻址空间是2^32, 即32个bit，也就是4个字节。同理64位编译器）
//short int : 2个字节
// int： 4个字节
//unsigned int : 4个字节
//float: 4个字节
//double: 8个字节
//long: 4个字节
//long long: 8个字节
//unsigned long: 4个字节
typedef union _Upload_Data_
{
unsigned char buffer[PROTOCL_DATA_SIZE];
struct _Sensor_Str_
{
unsigned int Header;
int InitParameter; //参数初始化标志位
int Base_Type; //车子类型
int Motor_Drive_Type; //电机驱动类型
int Pwm_Max; //PWM最大值
int Pwm_Min; //PWM最大值
int Max_Rpm; //电机最大转速
int Pwm_Bits; //PWM分辨率
int Motor_Reduction_Ratio; //电机减速比
int Encoder_Line_Number; //编码器分辨率
int Counts_Per_Rev; //编码器转动一周的计数值
int DebunMotor; //电机调试标志位
float Kp_; //PID的p参数
float Ki_; //PID的i参数
int Kd_; //PID的i参数
int PID_Debug_Enable; //PID动态调参使能标准位
float Wheel_Diameter; //轮子直径
float Lr_Wheels_Distance; //机器人左右轮子距离
float Fr_Wheels_Distance; //机器人前后轮子距离
int Direction_Motor1_Rotation; //电机转动方向标志位
int Direction_Motor2_Rotation; //电机转动方向标志位
int Direction_Motor3_Rotation; //电机转动方向标志位
int Direction_Motor4_Rotation; //电机转动方向标志位
int Direction_Encoder1_Value; //编码器计数方向标志位
int Direction_Encoder2_Value; //编码器计数方向标志位
int Direction_Encoder3_Value; //编码器计数方向标志位
int Direction_Encoder4_Value; //编码器计数方向标志位
long long Encoder1_Value; //编码器计数值
long long Encoder2_Value; //编码器计数值
long long Encoder3_Value; //编码器计数值
long long Encoder4_Value; //编码器计数值
float X_Speed; //右手坐标系的正解速度信息X
float Y_Speed; //右手坐标系的正解速度信息Y
float Z_Speed; //右手坐标系的正解速度信息Z
float Voltage; //电池当前电压
float Electricity; //电池输出电流
int Employ_Electricity; //已经使用电量
int Dump_Energy; //剩余电量
float Battery_Voltage_Used; //使用的电池电压值12或者24
int IMU_Type; //使用的IMU类型
IMU_Data Link_Accelerometer; //IMU的三轴加速度原始数据
IMU_Data Link_Gyroscope; //IMU的三轴角速度原始数据
IMU_Data Link_Magnetometer; //IMU的三轴磁力计原始数据
int Ultrasonic1_Enable; //超声波1使能标志位
int Ultrasonic2_Enable; //超声波2使能标志位
int Ultrasonic3_Enable; //超声波3使能标志位
float Ultrasonic1_Data; //超声波1数据
float Ultrasonic2_Data; //超声波2数据
float Ultrasonic3_Data; //超声波3数据
int Servo1_Enable; //舵机使能标志位
int Servo2_Enable; //舵机使能标志位
int Servo3_Enable; //舵机使能标志位
int Servo4_Enable; //舵机使能标志位
int Servo1_Data; //舵机转动角度值
int Servo2_Data; //舵机转动角度值
int Servo3_Data; //舵机转动角度值
int Servo4_Data; //舵机转动角度值
int Max_Steering_Angle; //舵机最大转动角度值
int Initial_Turning_Angle; //舵机初始化角度值
int Esc_Median_Value; //电调中值
int A2_Encoder_Enable; //A2车型编码器使能标志位
int Emergency_Stop_Enable; //急停按钮使能标志位
unsigned char End_flag; //包尾结束标志
}Sensor_Str;//55*4+12*3+8*4+1 =289
}Upload_Data;
#pragma pack(4)
class starrobot_robot_object
{
public:
starrobot_robot_object();
~starrobot_robot_object();
//
void base_InitParam();
void cmd_velCallback(const geometry_msgs::Twist &twist_aux);
void swerveCallback(const std_msgs::Int16& swerve_servo);
void pid_velCallback(const starrobot_msgs::PID& pid);
void servo_velCallback(const starrobot_msgs::Servo& servo_data);
void PublisherRawVel(void);
void PublisherRawImu(void);
void PublisherBattery(void);
void PublisherSonar(void);
void DebugEncoderDataShow(void);
void ReadAndWriteLoopProcess(void);
// Read/Write data from ttyUSB
bool ReadFormUart();
bool WriteToUart(unsigned char*){}
serial::Serial Robot_Serial; //声明串口对象
private:
int baud_data;
string usart_port,cmd_vel_sub_name_;
string swerve_sub_name_,pid_sub_name_,servo_sub_name_;
string raw_vel_pub_name_,raw_imu_pub_name_,battery_pub_name_,sonar_pub_name_;
string starrobot_motor_drive_,starrobot_base_,imu_type_;
// Ros node define
ros::NodeHandle nh_;
ros::Time current_time, last_time;
ros::Subscriber swerve_sub,pid_sub,servo_sub,cmd_vel_sub;
ros::Publisher raw_vel_pub,raw_imu_pub,battery_pub,sonar_pub;
starrobot_msgs::Imu raw_imu_msg;
starrobot_msgs::Velocities raw_vel_msg;
starrobot_msgs::Battery raw_bat_msg;
starrobot_msgs::Sonar raw_sonar_msg;
tf::TransformBroadcaster odom_broadcaster;
//
Upload_Data Reciver_Str, Send_Str;
//sensor_msgs::Imu IMU_Data;
//init base parameter
float lr_wheels_distance_,fr_wheels_distance_,wheel_diameter_;
//init motor parameter
int max_rpm_,pwm_bits_,pwm_max_,pwm_min_;
int motor_reduction_ratio_,encoder_line_number_,counts_per_rev_;
int Direction_motor1_rotation_,Direction_motor2_rotation_;
int Direction_motor3_rotation_,Direction_motor4_rotation_;
int Direction_encoder1_value_,Direction_encoder2_value_;
int Direction_encoder3_value_,Direction_encoder4_value_;
//init ultrasonic parameter
int ultrasonic1_Enable_,ultrasonic2_Enable_,ultrasonic3_Enable_;
//init servo parameter
int servo1_Enable_,servo2_Enable_,servo3_Enable_,servo4_Enable_;
int initial_turning_angle_,max_steering_angle_;
int esc_median_value_,a2_encoder_;
// init emergency stop parameter
int emergency_stop_;
//init PID parameter
float k_p_,k_i_,k_d_;
// debug parameter
int debug_message_,debug_base_;
//
int base_data_num,imu_data_num,motor_data_num,wheel_num;
};
#endif
自定义的数据类型上层和底层要相同
typedef union _Upload_Data_
{
unsigned char buffer[PROTOCL_DATA_SIZE];
struct _Sensor_Str_
{
unsigned int Header;
int InitParameter; //参数初始化标志位
int Base_Type; //车子类型
int Motor_Drive_Type; //电机驱动类型
int Pwm_Max; //PWM最大值
int Pwm_Min; //PWM最大值
int Max_Rpm; //电机最大转速
int Pwm_Bits; //PWM分辨率
int Motor_Reduction_Ratio; //电机减速比
int Encoder_Line_Number; //编码器分辨率
int Counts_Per_Rev; //编码器转动一周的计数值
int DebunMotor; //电机调试标志位
float Kp_; //PID的p参数
float Ki_; //PID的i参数
int Kd_; //PID的i参数
int PID_Debug_Enable; //PID动态调参使能标准位
float Wheel_Diameter; //轮子直径
float Lr_Wheels_Distance; //机器人左右轮子距离
float Fr_Wheels_Distance; //机器人前后轮子距离
int Direction_Motor1_Rotation; //电机转动方向标志位
int Direction_Motor2_Rotation; //电机转动方向标志位
int Direction_Motor3_Rotation; //电机转动方向标志位
int Direction_Motor4_Rotation; //电机转动方向标志位
int Direction_Encoder1_Value; //编码器计数方向标志位
int Direction_Encoder2_Value; //编码器计数方向标志位
int Direction_Encoder3_Value; //编码器计数方向标志位
int Direction_Encoder4_Value; //编码器计数方向标志位
long long Encoder1_Value; //编码器计数值
long long Encoder2_Value; //编码器计数值
long long Encoder3_Value; //编码器计数值
long long Encoder4_Value; //编码器计数值
float X_Speed; //右手坐标系的正解速度信息X
float Y_Speed; //右手坐标系的正解速度信息Y
float Z_Speed; //右手坐标系的正解速度信息Z
float Voltage; //电池当前电压
float Electricity; //电池输出电流
int Employ_Electricity; //已经使用电量
int Dump_Energy; //剩余电量
float Battery_Voltage_Used; //使用的电池电压值12或者24
int IMU_Type; //使用的IMU类型
IMU_Data Link_Accelerometer; //IMU的三轴加速度原始数据
IMU_Data Link_Gyroscope; //IMU的三轴角速度原始数据
IMU_Data Link_Magnetometer; //IMU的三轴磁力计原始数据
int Ultrasonic1_Enable; //超声波1使能标志位
int Ultrasonic2_Enable; //超声波2使能标志位
int Ultrasonic3_Enable; //超声波3使能标志位
float Ultrasonic1_Data; //超声波1数据
float Ultrasonic2_Data; //超声波2数据
float Ultrasonic3_Data; //超声波3数据
int Servo1_Enable; //舵机使能标志位
int Servo2_Enable; //舵机使能标志位
int Servo3_Enable; //舵机使能标志位
int Servo4_Enable; //舵机使能标志位
int Servo1_Data; //舵机转动角度值
int Servo2_Data; //舵机转动角度值
int Servo3_Data; //舵机转动角度值
int Servo4_Data; //舵机转动角度值
int Max_Steering_Angle; //舵机最大转动角度值
int Initial_Turning_Angle; //舵机初始化角度值
int Esc_Median_Value; //电调中值
int A2_Encoder_Enable; //A2车型编码器使能标志位
int Emergency_Stop_Enable; //急停按钮使能标志位
unsigned char End_flag; //包尾结束标志
}Sensor_Str;//55*4+12*3+8*4+1 =289
}Upload_Data;参数配置文件和上面的xxx.yaml文件相同，加载方式也是一样的。
这样就可以完成上层对底层参数的修改。
最后打个广告淘宝店铺链接：https://item.taobao.com/item.htm?spm=a1z10.1-c.w137644-21142386785.38.88736177LoLcDa&id=606445592295
交流群：
