1024问

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

Onida ·
更新时间:2024-11-10
· 652 次阅读

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

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 #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("port", this->usart_port, "/dev/starrobot_base"); nh_private.param("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(raw_vel_pub_name_, 10); raw_imu_pub = nh_.advertise(raw_imu_pub_name_, 10); battery_pub = nh_.advertise(battery_pub_name_, 10); sonar_pub = nh_.advertise(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 #include #include #include #include #include #include #include #include #include #include #include #include #include #include
1024 个赞
编辑 举报
需要 登录 后方可回复, 如果你还没有账号请 注册新账号
相关文章
JavaScript 对象
Maha 2021-04-02
903
css选择器设置标签样式的实例代码
Hope 2021-03-01
515
visual studio 2012安装配置方法图文教程 附opencv配置教程
Nafisa 2021-04-13
585
在ASP.NET 2.0中操作数据之六十一：在事务里对数据库修改进行封装
Elizabeth 2020-01-29
590
详解CentOS7 FTP服务搭建（虚拟用户访问FTP服务）
Lida 2020-09-12
865
C#实现串口通信的示例详解
Thalia 2023-05-12
459
ROS1 rosbag的详细使用并且使用python合并bag包的方法
Xanthe 2023-05-12
1482
C++初阶教程之缺省参数与函数重载
Hester 2023-05-12
1789
js解决url传递中文参数乱码问题的方法详解
Octavia 2023-05-12
932
spark大数据任务提交参数的优化记录分析
Anna 2023-05-13
776
使用React.forwardRef传递泛型参数
Anna 2023-05-16
1932
深入探究一下Java中不同的线程间数据通信方式
Kathy 2023-05-21
979
python浅谈一下线程间通信之队列
Tanisha 2023-07-01
1201
MYSQL数据库连接池及常见参数调优方式
Malina 2023-07-19
1117
使用AJAX异步通信技术实现搜索联想和自动补全示例
Tani 2023-07-20
1222
Shell脚本传递参数的4种方式实例说明
Bunny 2023-07-20
1813
检查Bash中是否存在输入参数的问题
Willow 2023-07-20
380
Python中pytest的参数化实例解析
Crystal 2023-07-20
1338
vue中watch监听路由传来的参数变化问题
Viridis 2023-07-20
822
SpringMVC和SpringBoot接收参数的几种方式详解
Lida 2023-07-21
1545