主程式:幾個重點部分

1.     加入檔頭#include "serial_reader.hpp"

2.     裡面就做 rclcapp 的操作, 不能另外宣告過跟構建函數一樣的 class 以免報錯.

       rclcpp::init(argc,argv); 

       autoSerial_controller_node = std::make_shared();

   rclcpp::spin(Serial_controller_node);

   rclcpp::shutdown();

3.    这功能是创见定时器,用來定時讀取串口timer_ = this->create_wall_timer(

4.    Control_loop 是讀取動作的call back

5.    if(count==1)這裡是避免做兩次declare_parameter或 get_parameter發生錯誤的walk around.

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include "serial_reader.hpp"

#include "rclcpp/rclcpp.hpp"

class SerialReaderNode : public rclcpp::Node

{

public:

serial::Serial Stm32_Serial;

SerialReaderNode(): Node("Serial_ctl_node")

{

timer_ = this->create_wall_timer(

std::chrono::milliseconds(1000),

std::bind(&SerialReaderNode::control_loop, this)

);

}

private:

serial::Serial serial_;

int serial_baud_rate;   //Serial communication baud rate //串口通信波特率

SEND_DATA Send_Data;   //The serial port sends the data structure //串口发送数据结构体

RECEIVE_DATA Receive_Data; //The serial port receives the data structure //串口接收数据结构体

RECEIVE_AutoCharge_DATA Receive_AutoCharge_Data;  //串口接收自动回充数据结构体

Vel_Pos_Data Robot_Pos;    //The position of the robot //机器人的位置

Vel_Pos_Data Robot_Vel;    //The speed of the robot //机器人的速度

MPU6050_DATA Mpu6050_Data; //IMU data //IMU数据

float Power_voltage;   //Power supply voltage //电源电压

rclcpp::Time _Now, _Last_Time;

float Sampling_Time;        //Sampling time, used for integration to find displacement (mileage) //采样时间，用于积分求位移(里程)

string usart_port_name, robot_frame_id, gyro_frame_id, odom_frame_id, akm_cmd_vel, test; //Define the related variables //定义相关变量

std::string cmd_vel;

int count=1;

short IMU_Trans(uint8_t Data_High,uint8_t Data_Low)

{

  short transition_16;

  transition_16 = 0;

  transition_16 |= Data_High<<8;

  transition_16 |= Data_Low;

  return transition_16;

}

float Odom_Trans(uint8_t Data_High,uint8_t Data_Low)

{

  float data_return;

  short transition_16;

  transition_16 = 0;

  transition_16 |= Data_High<<8;  //Get the high 8 bits of data  //获取数据的高8位

  transition_16 |= Data_Low;   //Get the lowest 8 bits of data //获取数据的低8位

  data_return =  (transition_16 / 1000)+(transition_16 % 1000)*0.001; // The speed unit is changed from mm/s to m/s //速度单位从mm/s转换为m/s

  return data_return;

}

unsigned char Check_Sum(unsigned char Count_Number,unsigned char mode)

{

  unsigned char check_sum=0,k;

  if(mode==0) //Receive data mode //接收数据模式

  {

  for(k=0;k<Count_Number;k++)

    {

    check_sum=check_sum^Receive_Data.rx[k]; //By bit or by bit //按位异或

    }

  }

  if(mode==1) //Send data mode //发送数据模式

  {

  for(k=0;k<Count_Number;k++)

    {

    check_sum=check_sum^Send_Data.tx[k]; //By bit or by bit //按位异或

    }

  }

  return check_sum; //Returns the bitwise XOR result //返回按位异或结果

}

void control_loop()

{ // 循环控制逻辑

RCLCPP_INFO(this->get_logger(), "在循环中执行控制逻辑 >>> %d",count);

if (count==1)

{

init_serial();

}

count++;

process_serial();

if (count>50000)

count=3;

}

bool init_serial()

{

serial_baud_rate = 115200;

this->declare_parameter<int>("serial_baud_rate");

this->declare_parameter<std::string>("usart_port_name", "/dev/ttyACM0");

this->declare_parameter<std::string>("cmd_vel", "cmd_vel");

this->declare_parameter<std::string>("akm_cmd_vel", "ackermann_cmd");

this->declare_parameter<std::string>("odom_frame_id", "odom");

this->declare_parameter<std::string>("robot_frame_id", "base_link");

this->declare_parameter<std::string>("gyro_frame_id", "gyro_link");

this->get_parameter("serial_baud_rate", serial_baud_rate);

this->get_parameter("usart_port_name", usart_port_name);

this->get_parameter("cmd_vel", cmd_vel);

this->get_parameter("akm_cmd_vel", akm_cmd_vel);

this->get_parameter("odom_frame_id", odom_frame_id);

this->get_parameter("robot_frame_id", robot_frame_id);

this->get_parameter("gyro_frame_id", gyro_frame_id);

try

{

// Attempts to initialize and open the serial port //尝试初始化与开启串口

  Stm32_Serial.setPort(usart_port_name); //Select the serial port number to enable //选择要开启的串口号

  Stm32_Serial.setBaudrate(serial_baud_rate); //Set the baud rate //设置波特率

  serial::Timeout _time = serial::Timeout::simpleTimeout(2000); //Timeout //超时等待

  Stm32_Serial.setTimeout(_time);

  Stm32_Serial.open(); //Open the serial port //开启串口

}

catch (serial::IOException& e)

{

  RCLCPP_ERROR(this->get_logger(),"Can not open serial port,Please check the serial port cable! "); //If opening the serial port fails, an error message is printed //如果开启串口失败，打印错误信息

return false;

}

if(Stm32_Serial.isOpen())

{

  RCLCPP_INFO(this->get_logger(),"serial port opened"); //Serial port opened successfully //串口开启成功提示

}

return true;

}

void process_serial()

{

rclcpp::Time current_time, last_time;

current_time = rclcpp::Node::now();

last_time = rclcpp::Node::now();

//Retrieves time interval, which is used to integrate velocity to obtain displacement (mileage)

//获取时间间隔，用于积分速度获得位移(里程)

Sampling_Time = (current_time - last_time).seconds();

//The serial port reads and verifies the data sent by the lower computer, and then the data is converted to international units

//通过串口读取并校验下位机发送过来的数据，然后数据转换为国际单位

if (true == Get_Sensor_Data())

{

  //Calculate the displacement in the X direction, unit: m //计算X方向的位移，单位：m

  Robot_Pos.X+=(Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time;

  //Calculate the displacement in the Y direction, unit: m //计算Y方向的位移，单位：m

  Robot_Pos.Y+=(Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time;

  //The angular displacement about the Z axis, in rad //绕Z轴的角位移，单位：rad

  Robot_Pos.Z+=Robot_Vel.Z * Sampling_Time;

}

// RCLCPP_INFO(this->get_logger(),"WHT read XYZ = %f , %f , %f ",Robot_Pos.X,Robot_Pos.Y,Robot_Pos.Z); //If opening the serial port fails, an error message is printed //如果开启串口失败，打印错误信息

}

bool Get_Sensor_Data()

{

  short transition_16=0, j=0, Header_Pos=0, Tail_Pos=0; //Intermediate variable //中间变量

  uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE]={0}; //Temporary variable to save the data of the lower machine //临时变量，保存下位机数据

  Stm32_Serial.read(Receive_Data_Pr,sizeof (Receive_Data_Pr)); //Read the data sent by the lower computer through the serial port //通过串口读取下位机发送过来的数据

//Record the position of the head and tail of the frame //记录帧头帧尾位置

  for(j=0;j<24;j++)

  {

if(Receive_Data_Pr[j]==FRAME_HEADER)

Header_Pos=j;

else if(Receive_Data_Pr[j]==FRAME_TAIL)

Tail_Pos=j;   

  }

  RCLCPP_INFO(this->get_logger(),"WHT read XYZ = %x , %x , %x ",Receive_Data_Pr[5],Receive_Data_Pr[10],Receive_Data_Pr[15]); //If opening the serial port fails, an error message is printed //如果开启串口失败，打印错误信息

  if(Tail_Pos==(Header_Pos+23))

  {

//If the end of the frame is the last bit of the packet, copy the packet directly to receive_data.rx

//如果帧尾在数据包最后一位，直接复制数据包到Receive_Data.rx

// ROS_INFO("1----");

memcpy(Receive_Data.rx, Receive_Data_Pr, sizeof(Receive_Data_Pr));

  }

  else if(Header_Pos==(1+Tail_Pos))

  {

//如果帧头在帧尾后面，纠正数据位置后复制数据包到Receive_Data.rx

// If the header is behind the end of the frame, copy the packet to receive_data.rx after correcting the data location

// ROS_INFO("2----");

for(j=0;j<24;j++)

Receive_Data.rx[j]=Receive_Data_Pr[(j+Header_Pos)%24];

  }

  else

  {

//其它情况则认为数据包有错误

// In other cases, the packet is considered to be faulty

RCLCPP_ERROR(this->get_logger(),"Serial Package failed !"); //If opening the serial port fails, an error message is printed //如果开启串口失败，打印错误信息

// return false;

  }   

  Receive_Data.Frame_Header= Receive_Data.rx[0]; //The first part of the data is the frame header 0X7B //数据的第一位是帧头0X7B

  Receive_Data.Frame_Tail= Receive_Data.rx[23];  //The last bit of data is frame tail 0X7D //数据的最后一位是帧尾0X7D

  if (Receive_Data.Frame_Header == FRAME_HEADER ) //Judge the frame header //判断帧头

  {

if (Receive_Data.Frame_Tail == FRAME_TAIL) //Judge the end of the frame //判断帧尾

{

  //BBC check passes or two packets are interlaced //BBC校验通过或者两组数据包交错

  if (Receive_Data.rx[22] == Check_Sum(22,READ_DATA_CHECK)||(Header_Pos==(1+Tail_Pos)))

  {

Receive_Data.Flag_Stop=Receive_Data.rx[1]; //set aside //预留位

//Get the speed of the moving chassis in the X direction //获取运动底盘X方向速度

Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]);

//Get the speed of the moving chassis in the Y direction, The Y speed is only valid in the omnidirectional mobile robot chassis

Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]);

//获取运动底盘Y方向速度，Y速度仅在全向移动机器人底盘有效

Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]); //Get the speed of the moving chassis in the Z direction //获取运动底盘Z方向速度 

RCLCPP_INFO(this->get_logger(),"WHT Chisass XYZ = %f , %f , %f ",Robot_Pos.X,Robot_Pos.Y,Robot_Pos.Z); //If opening the serial port fails, an error message is printed //如果开启串口失败，打印错误信息

//MPU6050 stands for IMU only and does not refer to a specific model. It can be either MPU6050 or MPU9250

//Mpu6050仅代表IMU，不指代特定型号，既可以是MPU6050也可以是MPU9250

Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]); //Get the X-axis acceleration of the IMU   //获取IMU的X轴加速度 

Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]); //Get the Y-axis acceleration of the IMU   //获取IMU的Y轴加速度

Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]); //Get the Z-axis acceleration of the IMU   //获取IMU的Z轴加速度

Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]);  //Get the X-axis angular velocity of the IMU //获取IMU的X轴角速度 

Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]);  //Get the Y-axis angular velocity of the IMU //获取IMU的Y轴角速度 

Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]);  //Get the Z-axis angular velocity of the IMU //获取IMU的Z轴角速度 

/*

//Linear acceleration unit conversion is related to the range of IMU initialization of STM32, where the range is ±2g=19.6m/s^2

//线性加速度单位转化，和STM32的IMU初始化的时候的量程有关,这里量程±2g=19.6m/s^2

Mpu6050.linear_acceleration.x = Mpu6050_Data.accele_x_data / ACCEl_RATIO;

Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data / ACCEl_RATIO;

Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data / ACCEl_RATIO;

//The gyroscope unit conversion is related to the range of STM32's IMU when initialized. Here, the range of IMU's gyroscope is ±500°/s

//Because the robot generally has a slow Z-axis speed, reducing the range can improve the accuracy

//陀螺仪单位转化，和STM32的IMU初始化的时候的量程有关，这里IMU的陀螺仪的量程是±500°/s

//因为机器人一般Z轴速度不快，降低量程可以提高精度

Mpu6050.angular_velocity.x =  Mpu6050_Data.gyros_x_data * GYROSCOPE_RATIO;

Mpu6050.angular_velocity.y =  Mpu6050_Data.gyros_y_data * GYROSCOPE_RATIO;

Mpu6050.angular_velocity.z =  Mpu6050_Data.gyros_z_data * GYROSCOPE_RATIO;

*/

//Get the battery voltage

//获取电池电压

transition_16 = 0;

transition_16 |=  Receive_Data.rx[20]<<8;

transition_16 |=  Receive_Data.rx[21];

Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001; //Unit conversion millivolt(mv)->volt(v) //单位转换毫伏(mv)->伏(v)

return true;

}

}

  }

  return false;

}

rclcpp::TimerBase::SharedPtr timer_;

};

int main(int argc, char** argv) 

{ 

rclcpp::init(argc, argv); 

auto Serial_controller_node = std::make_shared<SerialReaderNode>();

    rclcpp::spin(Serial_controller_node);

    rclcpp::shutdown();

return 0; 

}