ROS机器人DIY教程：ROS和STM32通信（常规通信方式和rosserial）之通过上层修改底层参数

本文主要介绍怎么编写代码实现在上层修改底层参数的相关知识，主要讲解使用两种通信方式来实现

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与GPIOA9GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;                       //复用功能GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;                //速度50MHzGPIO_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发送  TXDMA_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); //启动DMAUSART_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;                           //右手坐标系的正解速度信息Xfloat Y_Speed;                           //右手坐标系的正解速度信息Yfloat Z_Speed;                           //右手坐标系的正解速度信息Zfloat 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 msgsSend_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 msgsSend_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 msgsSend_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 msgsSend_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        40const 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;                           //右手坐标系的正解速度信息Xfloat Y_Speed;                           //右手坐标系的正解速度信息Yfloat Z_Speed;                           //右手坐标系的正解速度信息Zfloat Voltage;                           //电池当前电压          float Electricity;                       //电池输出电流int Employ_Electricity;                  //已经使用电量int Dump_Energy;                         //剩余电量        float Battery_Voltage_Used;              //使用的电池电压值12或者24int 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 defineros::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 parameterint 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 parameterint ultrasonic1_Enable_,ultrasonic2_Enable_,ultrasonic3_Enable_;//init servo parameterint servo1_Enable_,servo2_Enable_,servo3_Enable_,servo4_Enable_;int initial_turning_angle_,max_steering_angle_;int esc_median_value_,a2_encoder_;// init emergency stop parameterint emergency_stop_;//init PID parameterfloat k_p_,k_i_,k_d_;//  debug parameterint 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;                           //右手坐标系的正解速度信息Xfloat Y_Speed;                           //右手坐标系的正解速度信息Yfloat Z_Speed;                           //右手坐标系的正解速度信息Zfloat Voltage;                           //电池当前电压          float Electricity;                       //电池输出电流int Employ_Electricity;                  //已经使用电量int Dump_Energy;                         //剩余电量        float Battery_Voltage_Used;              //使用的电池电压值12或者24int 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

交流群：

ROS机器人DIY教程：ROS和STM32通信（常规通信方式和rosserial）之通过上层修改底层参数相关推荐

ROS机器人DIY教程：超声波数据获取（HC-SR04/US-100）
简介: 在机器人开发中虽然我们有激光雷达去感知周围的障碍物,但有的时候应为激光雷达安装位置的原因,无法识别到较低位置的障碍物,此时就需要一个性价比较高的传感器来进行辅助,一般情况可以使用红外或者超声波 ...
ROS入门21讲 | ROS机器人入门教程【简明笔记】
古月·ROS入门21讲 | 一学就会的ROS机器人入门教程文章目录 ROS核心概念 ROS命令行工作空间与功能包订阅与发布发布者 Publisher 订阅者 Subscriber 话题消息的自 ...
ROS机器人入门教程（1）
ROS WIKI 教程 Documentation - ROS Wiki 1. Linux系统基础操作安装ROS之前要修改软件有源,家庭网一般用阿里源,学校网一般用.edu结尾的. CTRL + a ...
ROS机器人开发实践——ROS自定义消息
1.在catkin_ws/src文件夹下面创建一个功能包 catkin_create_pkg learning_topic roscpp rospy std_msgs geometry_msgs 2. ...
关于RikiRobot ROS机器人的介绍
大家好,我是Rikirobot ROS机器人的开发者,风野萧萧,很多ROS新手或者买家对会有疑问,关于ROS系统怎么学习?ROS怎么入门?你店里的产品与其它的类似于Turtlebot2/3这种机器人有 ...
agx 安装ros opencv_【树莓派ROS开源机器人】阿克曼转向机器人，最接近无人自动驾驶的人工智能机器人...
塔克创新出品的系列入门ROS智能机器人学习开发平台,包括麦克纳姆轮机器人,四轮差速机器人,两轮差速机器人,阿克曼转向小车等平台.可实现SLAM建图导航.自动避障.雷达跟随.视觉巡线.三维建图.手机AP ...
ROS机器人项目开发11例-ROS Robotics Projects（10）机器人Web工具集
本书最后一章的内容是通过Web网页对机器人进行监测和控制等,使用的是Robot Web Tools. 插播一组视频,暑期学校的全部视频资料,可看: http://i.youku.com/i/UMTMx ...
ROS机器人程序设计（原书第2版）补充资料教学大纲
ROS机器人程序设计(原书第2版) 补充资料教学大纲针对该书稍后会补充教学大纲.教案.多媒体课件以及练习题等. <ROS机器人程序设计>课程简介课程编号:XXXXXX 课程名称:RO ...
【ROS wiki】ros wiki官方教程与ROS wiki页面检索
ROS wiki系列文章简介:ROS wiki系列文章是本人ROS专栏下的子专题.该系列文章主要用来介绍:ROS初学者如何利用好ROS官方提供的ROS wiki平台,来查询ROS资料,了解ROS包的功 ...

ROS机器人DIY教程：ROS和STM32通信（常规通信方式和rosserial）之通过上层修改底层参数

ROS机器人DIY教程：ROS和STM32通信（常规通信方式和rosserial）之通过上层修改底层参数相关推荐

最新文章

热门文章