机器人CPP编程基础-03变量类型Variables Types

机器人CPP编程基础-02变量Variables


全文AI生成。


C++

#include<iostream>

using namespace std;

main()
{
    int a=10,b=35; // 4 bytes
    cout<<"Value of a : "<<a<<" Address of a : "<<&a <<endl;
    cout<<"Value of b : "<<b<<" Address of b : "<<&b <<endl;

    short s=20; //2 bytes
    cout <<"Value of s : "<< s << endl;

    float f1=20.03; //4 bytes
    double d1=50.55416416; //8 bytes
    cout <<"Value of f1 : "<< f1 << endl;
    cout <<"Value of d1 : "<< d1 << endl;

    char c1='A';
    cout<<c1<<endl;

    string s1="Hello Tridib";
    cout<<s1<<endl;

    string s2="Welcome to CPP !";
    cout<<s2<<endl;

    string combineStrings=s1+", "+s2;
    cout<<combineStrings<<endl;

    bool b1=true; //Boolean is true or false. FALSE IS ALWAYS ZERO, EVERY OTHER NUMBER IS TRUE.
    cout<<b1<<endl;
    b1=1515;
    cout<<b1<<endl;
    b1=-445;
    cout<<b1<<endl;
    b1=0;
    cout<<b1<<endl;

    //Positive numbers or unsigned numbers. Increases the positive range.
    unsigned short int u1=60445;
    cout<< u1 <<endl;

    const string myname="Tridib";
    cout << myname << endl;

}

该代码是用C++编写的,它包含了各种数据类型的声明和初始化,以及如何使用cout语句来打印这些变量的值和地址。下面是每行的详细解析:

以上就是对这段代码的详细解析。

  1. #include<iostream>: 包含输入/输出流的库。这使得程序可以使用输入和输出功能。
  2. using namespace std;: 声明使用std命名空间。std是标准缩写,表示标准库。这样,我们就可以直接使用标准库中的名字,而不用在每个名字前都写std::
  3. main(): 程序的主函数,所有C++程序都从这里开始执行。
  4. int a=10,b=35;: 声明两个整数变量a和b并初始化为10和35。
  5. cout<<"Value of a : "<<a<<" Address of a : "<<&a <<endl;: 使用cout打印"Value of a : "和变量a的值,然后打印"Address of a : "和变量a的地址。&操作符用于获取变量的内存地址。endl用于插入新行。
  6. cout<<"Value of b : "<<b<<" Address of b : "<<&b <<endl;: 与上述类似,但打印的是变量b的值和地址。
  7. short s=20;: 声明一个短整型变量s并初始化为20。在大多数系统上,short通常是2字节(16位)。
  8. cout <<"Value of s : "<< s << endl;: 使用cout打印"Value of s : "和变量s的值。
  9. float f1=20.03;: 声明一个单精度浮点型变量f1并初始化为20.03。在大多数系统上,float通常是4字节(32位)。
  10. double d1=50.55416416;: 声明一个双精度浮点型变量d1并初始化为50.55416416。在大多数系统上,double通常是8字节(64位)。
  11. cout <<"Value of f1 : "<< f1 << endl;: 使用cout打印"Value of f1 : "和变量f1的值。
  12. cout <<"Value of d1 : "<< d1 << endl;: 使用cout打印"Value of d1 : "和变量d1的值。
  13. char c1='A';: 声明一个字符变量c1并初始化为'A'。
  14. cout<<c1<<endl;: 使用cout打印变量c1的值,也就是字符'A'。
  15. string s1="Hello Tridib";: 声明一个字符串变量s1并初始化为"Hello Tridib"。
  16. cout<<s1<<endl;: 使用cout打印变量s1的值,也就是"Hello Tridib"。
  17. string s2="Welcome to CPP !";: 声明另一个字符串变量s2并初始化为"Welcome to CPP !"。
  18. cout<<s2<<endl;: 使用cout打印变量s2的值,也就是"Welcome to CPP !"。
  19. string combineStrings=s1+", "+s2;: 将s1和s2连接在一起,创建一个新的字符串combineStrings。
  20. cout<<combineStrings<<endl;: 使用cout打印变量combineStrings的值,也就是"Hello Tridib, Welcome to CPP !"。
  21. bool b1=true;: 声明一个布尔型变量b1并初始化为true。在C++中,布尔类型的值只能是true或false。
  22. cout<<b1<<endl;: 使用cout打印变量b1的值,也就是true。在C++中,输出布尔值时会自动转换为字符串"true"或"false"。
  23. b1=1515;: 将变量b1的值更改为整数1515。在C++中,布尔类型的值也可以是0或非0的任何整数。
  24. cout<<b1<<endl;: 使用cout打印变量b1的值,由于非零的整数会被视为true,所以这里输出的是true。
  25. b1=-445;: 将变量b1的值更改为-445。
  26. cout<<b1<<endl;: 使用cout打印变量b1的值,由于非零的整数会被视为true,所以这里

    输出的是true。

  27.  b1=0;: 将变量b1的值更改为0。

  28. cout<<b1<<endl;: 使用cout打印变量b1的值,由于布尔类型的值只能是true或false,所以这里输出的是false。
  29. unsigned short int u1=60445;: 声明一个无符号短整型变量u1并初始化为60445。在大多数系统上,unsigned short通常是2字节(16位)。
  30. cout<< u1 <<endl;: 使用cout打印变量u1的值。由于是无符号类型,所以可以输出大于255的整数。
  31. const string myname="Tridib";: 声明一个常量字符串变量myname并初始化为"Tridib"。const关键字表示该变量的值不能被修改。
  32. cout << myname << endl;: 使用cout打印变量myname的值,也就是"Tridib"。

C++是一种多范式编程语言,支持多种变量类型。下面是C++中主要的变量类型及其描述:

  1. 基本类型

    • 整数类型:这些类型用于存储整数值。有符号和无符号两种类型。

      • int: 通常为32位,但大小可能因平台而异。
      • short: 通常为16位。
      • long: 通常为32位或64位,大小可能因平台而异。
      • long long: 通常为64位,大小可能因平台而异。
    • 浮点类型:这些类型用于存储带有小数点的值。

      • float: 单精度,通常为32位。
      • double: 双精度,通常为64位。
    • 字符类型:这些类型用于存储单个字符或ASCII值。

      • char: 有符号或无符号,通常为8位。
    • 布尔类型bool,用于存储逻辑值,可以取truefalse

  2. 复合类型

    • 数组:用于存储相同类型的多个元素。
    • 字符串:字符数组,用于存储多个字符。
    • 结构体:用于将多个不同类型的变量组合在一起。
    • 联合体:用于在内存中共享多个不同数据类型的变量。
  3. 指针类型

    • int *p;:定义一个指向整数的指针。
    • double *dp;:定义一个指向双精度浮点数的指针。
    • char *cp;:定义一个指向字符的指针。
  4. 引用类型

    • int &ref;:定义一个对整数变量的引用。引用和它引用的变量在内存中是同一实体。
  5. 枚举类型

    • enum color { red, green, blue };:定义了一个名为color的枚举类型,包含三个可能的值:red、green和blue。
  6. 自定义类型

    • :定义自定义的数据类型和方法。类是一种用户自定义的数据类型,可以包含属性(变量)和方法(函数)。
  7. 特殊类型

    • void:表示无类型,常用于函数的返回值类型,如void func()表示该函数没有返回值。
    • const:表示常量,一旦给定值就不能改变。例如const int a = 10;,a的值就不能再改变。
    • volatile:表示易变的,即程序无法预知该变量的值会随时发生变化。例如硬件寄存器的值、正在运行的进程等。
  8. 复合类型(如数组、指针、引用等)的进一步扩展:

    • 指针数组:数组的元素是指针类型。例如int *arr[5]定义了一个包含5个指向整数的指针的数组。
    • 指向指针的指针:例如int **pp;定义了一个指向指针的指针,该指针可以指向一个整数类型的变量。
    • 引用数组:例如int &arr[5]定义了一个包含5个整数引用的数组。每个引用都是它引用的数组元素的别名。
  9. 函数指针:指向函数的指针,例如int (*fp)(int)定义了一个接受一个整数参数并返回一个整数的函数指针。

  10. 智能指针:C++11引入了智能指针,以帮助管理动态分配的内存。智能指针是一种类,它以一种安全的方式自动管理动态分配的内存。以下是一些智能指针类型:

    • std::unique_ptr<T>:这种类型的指针在离开作用域时自动删除其所有权。它只能指向一个对象,并保证其指向的对象在任何时刻都有一个所有者。
    • std::shared_ptr<T>:这种类型的指针允许多个智能指针共享同一个对象。当最后一个智能指针离开作用域或被重置时,它会自动删除所指向的对象。
    • std::weak_ptr<T>:这种类型的指针也是指向共享对象的,但不会增加对象的引用计数。当最后一个shared_ptr离开作用域或被重置时,weak_ptr将看到对象的析构。

这些是C++中的主要变量类型。根据需要,还可以创建自定义类型和结构,以满足特定的编程需求。

C++14引入了一些新的变量类型,这些类型在C++11和C++17中都有所扩展。以下是一些C++14中引入的新变量类型:

  1. 原子类型(Atomic Types):这些类型定义在<atomic>头文件中,用于表示可以在不引起其他线程阻塞的情况下被单独操作的值。原子类型包括整数、浮点数和指针类型。

    • atomic<T>:定义了一个原子操作的基本类型。原子操作是一种在单个线程中执行的操作,它可以在没有其他线程干扰的情况下修改数据。
    • atomic_flag:定义了一个原子标志,可以用于实现简单的原子布尔操作。
  2. 数组的初始化:C++14引入了一种新的数组初始化方法,允许使用花括号{}进行初始化。例如:

 

cpp复制代码

int arr[] = {1, 2, 3, 4, 5};
  1. 通用 Lambda 捕获:C++14引入了通用 lambda 捕获,允许捕获变量,无论它们是否在作用域中。例如:

 

cpp复制代码

auto lambda = [captured_var = some_global_var](){ /* Use captured_var */ };
  1. 右值引用(Rvalue References):进一步扩展了右值引用,使其更易于使用和更灵活。C++14引入了std::movestd::forward函数,用于移动语义和完美转发。
  2. 类型萃取(Type deduction):C++14引入了一些新的类型萃取规则,包括萃取数组和函数类型的参数。
  3. Unicode 支持:C++14引入了更全面的 Unicode 支持,包括新的字符类型和函数。
  4. 线程局部存储(Thread-local storage):C++14引入了线程局部存储,允许为每个线程存储单独的变量副本。
  5. 二进制字面值:C++14引入了新的二进制字面值,包括二进制整数和二进制浮点数。
  6. 字面值的显示浮点数:C++14允许在字面值中使用显示浮点数,例如0x1p+2n表示一个十六进制浮点数。

这些是C++14引入的一些主要变量类型和功能。这些功能增强了语言的表达力和性能,并提供了更多的灵活性和控制。

C++20引入了一些新的变量类型和功能,进一步扩展了语言的表达力和性能。以下是一些C++20中引入的新变量类型和功能:

  1. 概念(Concepts):概念是一种用于指定类型必须满足的条件的语言特性。可以使用概念来约束模板参数的类型,以便在模板实例化时确保类型符合特定的要求。这有助于提高代码的可读性和可维护性。

 

cpp复制代码

template <typename T> concept bool C() { return requires (T t) { t++; }; }
template <C T> void f(T t) { ++t; }
  1. 结构化绑定(Structured Binding):C++20引入了结构化绑定,允许将一个表达式的值直接分配给多个变量。这使得在解构数组、元组或其他复合类型时更加方便。

 

cpp复制代码

int a = 1, b = 2;
auto [c, d] = a + b; // c = 3, d = 3
  1. if constexpr:C++20引入了if constexpr语句,它允许在编译时根据条件选择不同的代码路径。这对于在编译时进行条件编译和静态分派非常有用。

 

cpp复制代码

template <typename T> void f(T value) {
if constexpr (std::is_integral_v<T>) {
// 处理整数类型
} else if constexpr (std::is_floating_point_v<T>) {
// 处理浮点数类型
}
}
  1. 三向比较(Three-way Comparison):C++20引入了三向比较运算符<=>,用于执行基于比较的操作。这可以用于实现自定义类型的比较操作。
  2. 模板别名(Template Aliases):C++20引入了模板别名,允许使用别名来简化模板的声明和使用。
  3. 折叠表达式(Fold Expressions):C++20引入了折叠表达式,允许在展开二元或三元运算符时进行递归计算。这对于实现高阶函数和泛型算法非常有用。
  4. 嵌套的命名空间(Nested Namespaces):C++20允许在命名空间内部定义另一个命名空间,以更好地组织代码。
  5. 上下文重复名称(Contextual Repeated Name):C++20允许在特定上下文中重复使用名称,这有助于提高代码的可读性。
  6. 模块(Modules):C++20引入了模块,这是一种将代码划分为逻辑单元的方式,以提高编译速度和代码组织。模块可以用于实现特定功能或库的封装和重用。
  7. 概念工具(Concept Tools):C++20提供了一组工具,用于定义和操作概念。这有助于提高代码的可读性和可维护性。

这些是C++20引入的一些主要变量类型和功能。这些功能进一步增强了语言的表达力和性能,并提供了更多的灵活性和控制。


Arduino

#include <avr/pgmspace.h>  
  
void setup() {  
  Serial.begin(9600);  
  
  int a = 10;  
  Serial.print("Value of a : ");  
  Serial.print(a);  
  Serial.print(" Address of a : ");  
  Serial.println(&a);  
  
  short s = 20;  
  Serial.print("Value of s : ");  
  Serial.println(s);  
  
  float f1 = 20.03;  
  Serial.print("Value of f1 : ");  
  Serial.print(f1);  
  Serial.println();  
  
  double d1 = 50.55416416;  
  Serial.print("Value of d1 : ");  
  Serial.println(d1);  
  
  char c1 = 'A';  
  Serial.print(c1);  
  Serial.println();  
  
  const char* s1 = "Hello Tridib";  
  Serial.println(s1);  
  
  const char* s2 = "Welcome to CPP !";  
  Serial.println(s2);  
  
  const char* combineStrings = pgm_read_word(&combineStrings); //pgm_read_word(&combineStrings) is used to get the address of the combineStrings variable in flash memory space and is not related to the original code  
  Serial.println(combineStrings);  
  
  bool b1 = true;  
  Serial.println(b1);  
  b1 = 1515;  
  Serial.println(b1);  
  b1 = -445;  
  Serial.println(b1);  
  b1 = 0;  
  Serial.println(b1);  
  
  unsigned short int u1 = 60445;  
  Serial.println(u1);  
}  
  
void loop() {  
  // put your main code here, to run repeatedly:  
}

Arduino IDE使用的是C++的变量类型,因此它支持C++的所有常见变量类型。以下是在Arduino IDE中常用的C++变量类型:

  1. 基本类型

    • int: 用于表示整数值,通常为16位。
    • unsigned int: 用于表示无符号整数值,通常为16位。
    • long: 用于表示长整数值,通常为32位。
    • unsigned long: 用于表示无符号长整数值,通常为32位。
    • char: 用于表示字符值,通常为8位。
    • bool: 用于表示布尔值,可以取truefalse
    • float: 用于表示单精度浮点数值,通常为32位。
    • double: 用于表示双精度浮点数值,通常为64位。
  2. 指针类型

    • int *p;:定义一个指向整数的指针。
    • char *str;:定义一个指向字符数组的指针。
  3. 数组类型

    • int arr[10];:定义一个包含10个整数的数组。
    • char str[] = "Hello";:定义一个包含字符串"Hello"的字符数组。
  4. 结构体类型

    • struct MyStruct { int id; char name[20]; };:定义一个名为MyStruct的结构体,包含一个整型成员变量id和一个字符数组成员变量name。
  5. 联合体类型

    • union MyUnion { int id; char name[20]; };:定义一个名为MyUnion的联合体,包含一个整型成员变量id和一个字符数组成员变量name。
  6. 函数类型

    • int myFunction(int arg1, char arg2);:定义一个名为myFunction的函数,接受一个整型参数arg1和一个字符型参数arg2,并返回一个整型值。
  7. 自定义类型

    • typedef int MyInt;:定义一个新的类型别名MyInt,相当于int类型。
  8. 枚举类型

    • enum MyEnum { RED, BLUE, GREEN };:定义一个名为MyEnum的枚举类型,包含三个可能的值:RED、BLUE和GREEN。
  9. 类型转换

    • 自动类型转换:将一种类型的值赋给另一种类型的变量时,编译器会自动进行类型转换。例如,将一个整数值赋给一个浮点变量时,编译器会将整数值转换为浮点数。
    • 强制类型转换:使用强制类型转换运算符将一种类型的值转换为另一种类型。例如,(float) x将x转换为浮点数类型。
  10. 常量和常量表达式

    • 常量:使用const关键字定义常量,其值在程序运行期间不可改变。例如,const int kConstantValue = 42;
    • 常量表达式:使用const和字面值初始化器定义常量表达式。常量表达式必须是可以在编译时计算出结果的表达式,不能包含变量或函数调用。例如,const int kConstantExpression = 4 * 7;是常量表达式,而const int kVariableExpression = k + 1;不是常量表达式,因为k的值在编译时无法确定。

这些是在Arduino IDE中常用的C++变量类型和相关概念。了解这些变量类型和概念对于编写Arduino程序非常重要。


ROS1

AI生成是否可行???

#include <ros/ros.h>  
#include <std_msgs/String.h>  
  
int main(int argc, char **argv)  
{  
    ros::init(argc, argv, "my_node");  
    ros::NodeHandle nh;  
  
    int a=10,b=35; // 4 bytes  
    ros::console::cout << "Value of a : " << a << " Address of a : " << &a << ros::console::endl;  
    ros::console::cout << "Value of b : " << b << " Address of b : " << &b << ros::console::endl;  
  
    short s=20; //2 bytes  
    ros::console::cout << "Value of s : " << s << ros::console::endl;  
  
    float f1=20.03; //4 bytes  
    ros::console::cout << "Value of f1 : " << f1 << ros::console::endl;  
  
    double d1=50.55416416; //8 bytes  
    ros::console::cout << "Value of d1 : " << d1 << ros::console::endl;  
  
    int c1 = 65;  
    ros::console::cout << c1 << ros::console::endl;  
  
    std_msgs::String s1;  
    s1.data = "Hello Tridib";  
    ros::console::cout << s1.data << ros::console::endl;  
  
    std_msgs::String s2;  
    s2.data = "Welcome to CPP !";  
    ros::console::cout << s2.data << ros::console::endl;  
  
    // string concatenation not supported in ROS  
    // you can use roscpp'sRosOutSignal to achieve this easily.  
    // RosOutSignal().outStr(s1.data + ", " + s2.data);   
    // instead of the line above you can do:  
    std_msgs::String combineStrings;  
    combineStrings.data = s1.data + ", " + s2.data;  
    ros::console::cout << combineStrings.data << ros::console::endl;  
  
    bool b1=true; //Boolean is true or false. FALSE IS ALWAYS ZERO, EVERY OTHER NUMBER IS TRUE.  
    ros::console::cout << b1 << ros::console::endl;  
    b1=1515;  
    ros::console::cout << b1 << ros::console::endl;  
    b1=-445;  
    ros::console::cout << b1 << ros::console::endl;  
    b1=0;  
    ros::console::cout << b1 << ros::console::endl;  
  
    //Positive numbers or unsigned numbers. Increases the positive range.  
    unsigned short int u1=60445;  
    ros::console::cout << u1 << ros::console::endl;  
  
    // const string myname="Tridib"; is not supported in ROS, you should use a string_view instead. (not implemented in roscpp at the time of writing)  
}

ROS1 Noetic是ROS1的一个版本,使用C++作为主要的编程语言。在ROS1 Noetic中,常见的C++变量类型包括以下几种:

  1. 基本类型:与标准C++相同,包括int、float、double、char等。
  2. 智能指针类型:ROS1 Noetic主要使用std::shared_ptrstd::unique_ptr两种智能指针类型,用于自动管理资源的生命周期。
  3. 回调函数类型:ROS1 Noetic提供了ROS1特定的回调函数类型,如boost::functionboost::slot,用于实现事件驱动的回调机制。
  4. 命名空间:ROS1 Noetic使用了多个命名空间,如rosstd_msgssensor_msgs等,用于组织和管理ROS相关的代码和消息类型。
  5. 消息类型:ROS1 Noetic使用特定的消息类型,如std_msgs::Stringsensor_msgs::Image等,用于在ROS系统中进行节点之间的通信。这些消息类型通常定义在ROS1的消息规范中。
  6. 服务类型:ROS1 Noetic使用特定的服务类型,如ros::ServiceServerros::ServiceResponse,用于在ROS系统中实现服务调用。这些服务类型定义在ROS1的服务规范中。
  7. 动作类型:ROS1 Noetic使用特定的动作类型,如actionlib::SimpleActionClientactionlib::SimpleGoal,用于在ROS系统中实现复杂的行为控制。这些动作类型定义在ROS1的动作规范中。

除了上述变量类型外,ROS1 Noetic还提供了许多其他的工具和库,用于实现机器人控制、传感器数据处理、可视化等功能。


ros2中晃动imu,但是我的机器人模型不跟着晃动,而且里程计一开,里程计跑走,但是imu和机器人模型还在原地,以下是我的各个文件代码,情分析一下为什么:wheeltec_robot.cpp文件代码为: #include "turn_on_wheeltec_robot/wheeltec_robot.h" #include "turn_on_wheeltec_robot/Quaternion_Solution.h" #include "wheeltec_robot_msg/msg/data.hpp" sensor_msgs::msg::Imu Mpu6050;//Instantiate an IMU object //实例化IMU对象 using std::placeholders::_1; using namespace std; rclcpp::Node::SharedPtr node_handle = nullptr; //自动回充使用相关变量 bool check_AutoCharge_data = false; bool charge_set_state = false; /************************************** Date: January 28, 2021 Function: The main function, ROS initialization, creates the Robot_control object through the Turn_on_robot class and automatically calls the constructor initialization 功能: 主函数,ROS初始化,通过turn_on_robot类创建Robot_control对象并自动调用构造函数初始化 ***************************************/ int main(int argc, char** argv) { rclcpp::init(argc, argv); //ROS initializes and sets the node name //ROS初始化 并设置节点名称 turn_on_robot Robot_Control;//Instantiate an object //实例化一个对象 Robot_Control.Control();//Loop through data collection and publish the topic //循环执行数据采集和发布话题等操作 return 0; } /************************************** Date: January 28, 2021 Function: Data conversion function 功能: 数据转换函数 ***************************************/ short turn_on_robot::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 turn_on_robot::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; } /************************************** Date: January 28, 2021 Function: The speed topic subscription Callback function, according to the subscribed instructions through the serial port command control of the lower computer 功能: 速度话题订阅回调函数Callback,根据订阅的指令通过串口发指令控制下位机 ***************************************/ void turn_on_robot::Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux) { short transition; //intermediate variable //中间变量 Send_Data.tx[0]= 0x06; //frame head 0x7B //帧头0X7B //Send_Data.tx[1] = AutoRecharge; //set aside //预留位 Send_Data.tx[1] = FRAME_HEADER; Send_Data.tx[2] = 0; //set aside //预留位 //The target velocity of the X-axis of the robot //机器人x轴的目标线速度 transition=0; transition = twist_aux->linear.x*1000; //将浮点数放大一千倍,简化传输 (mm/s) Send_Data.tx[4] = transition; //取数据的低8位 Send_Data.tx[3] = transition>>8; //取数据的高8位 //The target velocity of the Y-axis of the robot //机器人y轴的目标线速度 transition=0; transition = twist_aux->linear.y*1000; Send_Data.tx[6] = transition; Send_Data.tx[5] = transition>>8; //The target angular velocity of the robot's Z axis //机器人z轴的目标角速度 transition=0; transition = twist_aux->angular.z*1000; //(1000*rad/s) Send_Data.tx[8] = transition; Send_Data.tx[7] = transition>>8; Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //For the BCC check bits, see the Check_Sum function //BCC校验位,规则参见Check_Sum函数 Send_Data.tx[10]=0x0d; //frame tail 0x7D //帧尾0X7D Send_Data.tx[11]=0x0a; try { Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //Sends data to the downloader via serial port //通过串口向下位机发送数据 } catch (serial::IOException& e) { RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port")); //If sending data fails, an error message is printed //如果发送数据失败,打印错误信息 } } /************************************** Date: January 28, 2021 Function: Publish the IMU data topic 功能: 发布IMU数据话题 ***************************************/ void turn_on_robot::Publish_ImuSensor() { sensor_msgs::msg::Imu Imu_Data_Pub; //Instantiate IMU topic data //实例化IMU话题数据 Imu_Data_Pub.header.stamp = rclcpp::Node::now(); Imu_Data_Pub.header.frame_id = gyro_frame_id; //IMU corresponds to TF coordinates, which is required to use the robot_pose_ekf feature pack //IMU对应TF坐标,使用robot_pose_ekf功能包需要设置此项 Imu_Data_Pub.orientation.x = Mpu6050.orientation.x; //A quaternion represents a three-axis attitude //四元数表达三轴姿态 Imu_Data_Pub.orientation.y = Mpu6050.orientation.y; Imu_Data_Pub.orientation.z = Mpu6050.orientation.z; Imu_Data_Pub.orientation.w = Mpu6050.orientation.w; Imu_Data_Pub.orientation_covariance[0] = 1e6; //Three-axis attitude covariance matrix //三轴姿态协方差矩阵 Imu_Data_Pub.orientation_covariance[4] = 1e6; Imu_Data_Pub.orientation_covariance[8] = 1e-6; Imu_Data_Pub.angular_velocity.x = Mpu6050.angular_velocity.x; //Triaxial angular velocity //三轴角速度 Imu_Data_Pub.angular_velocity.y = Mpu6050.angular_velocity.y; Imu_Data_Pub.angular_velocity.z = Mpu6050.angular_velocity.z; Imu_Data_Pub.angular_velocity_covariance[0] = 1e6; //Triaxial angular velocity covariance matrix //三轴角速度协方差矩阵 Imu_Data_Pub.angular_velocity_covariance[4] = 1e6; Imu_Data_Pub.angular_velocity_covariance[8] = 1e-6; Imu_Data_Pub.linear_acceleration.x = Mpu6050.linear_acceleration.x; //Triaxial acceleration //三轴线性加速度 Imu_Data_Pub.linear_acceleration.y = Mpu6050.linear_acceleration.y; Imu_Data_Pub.linear_acceleration.z = Mpu6050.linear_acceleration.z; imu_publisher->publish(Imu_Data_Pub); //Pub IMU topic //发布IMU话题 } /************************************** Date: January 28, 2021 Function: Publish the odometer topic, Contains position, attitude, triaxial velocity, angular velocity about triaxial, TF parent-child coordinates, and covariance matrix 功能: 发布里程计话题,包含位置、姿态、三轴速度、绕三轴角速度、TF父子坐标、协方差矩阵 ***************************************/ void turn_on_robot::Publish_Odom() { //Convert the Z-axis rotation Angle into a quaternion for expression //把Z轴转角转换为四元数进行表达 tf2::Quaternion q; q.setRPY(0,0,Mpu6050_Data.imu_yaw); geometry_msgs::msg::Quaternion odom_quat=tf2::toMsg(q); nav_msgs::msg::Odometry odom; //Instance the odometer topic data //实例化里程计话题数据 odom.header.stamp = rclcpp::Node::now(); ; odom.header.frame_id = odom_frame_id; // Odometer TF parent coordinates //里程计TF父坐标 odom.pose.pose.position.x = Robot_Pos.X; //Position //位置 odom.pose.pose.position.y = Robot_Pos.Y; //odom.pose.pose.position.z = Robot_Pos.Z; odom.pose.pose.orientation = odom_quat; //Posture, Quaternion converted by Z-axis rotation //姿态,通过Z轴转角转换的四元数 odom.child_frame_id = robot_frame_id; // Odometer TF subcoordinates //里程计TF子坐标 odom.twist.twist.linear.x = Robot_Vel.X; //Speed in the X direction //X方向速度 //odom.twist.twist.linear.y = Robot_Vel.Y; //Speed in the Y direction //Y方向速度 odom.twist.twist.angular.z = Mpu6050.angular_velocity.z; //Angular velocity around the Z axis //绕Z轴角速度 //There are two types of this matrix, which are used when the robot is at rest and when it is moving.Extended Kalman Filtering officially provides 2 matrices for the robot_pose_ekf feature pack //这个矩阵有两种,分别在机器人静止和运动的时候使用。扩展卡尔曼滤波官方提供的2个矩阵,用于robot_pose_ekf功能包 //if(Robot_Vel.X== 0&&Robot_Vel.Y== 0&&Robot_Vel.Z== 0) if(Robot_Vel.X== 0) //If the velocity is zero, it means that the error of the encoder will be relatively small, and the data of the encoder will be considered more reliable //如果velocity是零,说明编码器的误差会比较小,认为编码器数据更可靠 memcpy(&odom.pose.covariance, odom_pose_covariance2, sizeof(odom_pose_covariance2)), memcpy(&odom.twist.covariance, odom_twist_covariance2, sizeof(odom_twist_covariance2)); else //If the velocity of the trolley is non-zero, considering the sliding error that may be brought by the encoder in motion, the data of IMU is considered to be more reliable //如果小车velocity非零,考虑到运动中编码器可能带来的滑动误差,认为imu的数据更可靠 memcpy(&odom.pose.covariance, odom_pose_covariance, sizeof(odom_pose_covariance)), memcpy(&odom.twist.covariance, odom_twist_covariance, sizeof(odom_twist_covariance)); odom_publisher->publish(odom); //Pub odometer topic //发布里程计话题 } /************************************** Date: January 28, 2021 Function: Publish voltage-related information 功能: 发布电压相关信息 ***************************************/ void turn_on_robot::Publish_Voltage() { std_msgs::msg::Float32 voltage_msgs; //Define the data type of the power supply voltage publishing topic //定义电源电压发布话题的数据类型 static float Count_Voltage_Pub=0; if(Count_Voltage_Pub++>10) { Count_Voltage_Pub=0; voltage_msgs.data = Power_voltage; //The power supply voltage is obtained //电源供电的电压获取 voltage_publisher->publish(voltage_msgs); //Post the power supply voltage topic unit: V, volt //发布电源电压话题单位:V、伏特 } } ////////// 回充发布与回调 //////// /************************************** Date: January 17, 2022 Function: Pub the topic whether the robot finds the infrared signal (charging station) 功能: 发布机器人是否寻找到红外信号(充电桩)的话题 ***************************************/ void turn_on_robot::Publish_RED() { std_msgs::msg::UInt8 msg; msg.data=Red; RED_publisher->publish(msg); } /************************************** Date: January 14, 2022 Function: Publish a topic about whether the robot is charging 功能: 发布机器人是否在充电的话题 ***************************************/ void turn_on_robot::Publish_Charging() { static bool last_charging; std_msgs::msg::Bool msg; msg.data=Charging; Charging_publisher->publish(msg); if(last_charging==false && Charging==true)cout<<GREEN<<"Robot is charging."<<endl<<RESET; if(last_charging==true && Charging==false)cout<<RED <<"Robot charging has disconnected."<<endl<<RESET; last_charging=Charging; } /************************************** Date: January 28, 2021 Function: Publish charging current information 功能: 发布充电电流信息 ***************************************/ void turn_on_robot::Publish_ChargingCurrent() { std_msgs::msg::Float32 msg; msg.data=Charging_Current; Charging_current_publisher->publish(msg); } /************************************** Date: March 1, 2022 Function: Infrared connection speed topic subscription Callback function, according to the subscription command through the serial port to set the infrared connection speed 功能: 红外对接速度话题订阅回调函数Callback,根据订阅的指令通过串口发指令设置红外对接速度 ***************************************/ void turn_on_robot::Red_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux) { short transition; //intermediate variable //中间变量 Send_Data.tx[0]=FRAME_HEADER; //frame head 0x7B //帧头0X7B Send_Data.tx[1] = 3; //Infrared docking speed setting flag bit = 3 //红外对接速度设置标志位=3 Send_Data.tx[2] = 0; //set aside //预留位 //The target velocity of the X-axis of the robot //机器人x轴的目标线速度 transition=0; transition = twist_aux->linear.x*1000; //将浮点数放大一千倍,简化传输 Send_Data.tx[4] = transition; //取数据的低8位 Send_Data.tx[3] = transition>>8; //取数据的高8位 //The target velocity of the Y-axis of the robot //机器人y轴的目标线速度 transition=0; transition = twist_aux->linear.y*1000; Send_Data.tx[6] = transition; Send_Data.tx[5] = transition>>8; //The target angular velocity of the robot's Z axis //机器人z轴的目标角速度 transition=0; transition = twist_aux->angular.z*1000; Send_Data.tx[8] = transition; Send_Data.tx[7] = transition>>8; Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //BCC check //BCC校验 Send_Data.tx[10]=FRAME_TAIL; //frame tail 0x7D //帧尾0X7D try { Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //Sends data to the downloader via serial port //通过串口向下位机发送数据 } catch (serial::IOException& e) { RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port")); //If sending data fails, an error message is printed //如果发送数据失败,打印错误信息 } } /************************************** Date: January 14, 2022 Function: Subscription robot recharge flag bit topic, used to tell the lower machine speed command is normal command or recharge command 功能: 订阅机器人是否回充标志位话题,用于告诉下位机速度命令是正常命令还是回充命令 ***************************************/ void turn_on_robot::Recharge_Flag_Callback(const std_msgs::msg::Int8::SharedPtr Recharge_Flag) { AutoRecharge=Recharge_Flag->data; } //服务 void turn_on_robot::Set_Charge_Callback(const shared_ptr<turtlesim::srv::Spawn::Request> req,shared_ptr<turtlesim::srv::Spawn::Response> res) { Send_Data.tx[0]=FRAME_HEADER; //frame head 0x7B //֡ͷ0X7B if(round(req->x)==1) Send_Data.tx[1] = 1; else if(round(req->x)==2) Send_Data.tx[1] = 2; else if(round(req->x)==0) Send_Data.tx[1] = 0,AutoRecharge=0; Send_Data.tx[2] = 0; Send_Data.tx[3] = 0; Send_Data.tx[4] = 0; Send_Data.tx[5] = 0; Send_Data.tx[6] = 0; Send_Data.tx[7] = 0; Send_Data.tx[8] = 0; Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //For the BCC check bits, see the Check_Sum function //BCCУ��λ������μ�Check_Sum���� Send_Data.tx[10]=FRAME_TAIL; //frame tail 0x7D //֡β0X7D try { Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //Sends data to the downloader via serial port //ͨ����������λ���������� } catch (serial::IOException& e) { res->name = "false"; } if( Send_Data.tx[1]==0 ) { if(charge_set_state==0) AutoRecharge=0,res->name = "true"; else res->name = "false"; } else { if(charge_set_state==1) res->name = "true"; else res->name = "false"; } return; } ////////// 回充发布与回调 //////// /************************************** Date: January 28, 2021 Function: Serial port communication check function, packet n has a byte, the NTH -1 byte is the check bit, the NTH byte bit frame end.Bit XOR results from byte 1 to byte n-2 are compared with byte n-1, which is a BCC check Input parameter: Count_Number: Check the first few bytes of the packet 功能: 串口通讯校验函数,数据包n有个字节,第n-1个字节为校验位,第n个字节位帧尾。第1个字节到第n-2个字节数据按位异或的结果与第n-1个字节对比,即为BCC校验 输入参数: Count_Number:数据包前几个字节加入校验 mode:对发送数据还是接收数据进行校验 ***************************************/ unsigned char turn_on_robot::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=1;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 //返回按位异或结果 } //自动回充专用校验位 unsigned char turn_on_robot::Check_Sum_AutoCharge(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_AutoCharge_Data.rx[k]; //By bit or by bit //按位异或 } } return check_sum; } /************************************** Date: November 18, 2021 Function: Read and verify the data sent by the lower computer frame by frame through the serial port, and then convert the data into international units 功能: 通过串口读取并逐帧校验下位机发送过来的数据,然后数据转换为国际单位 ***************************************/ bool turn_on_robot::Get_Sensor_Data_New() { short transition_16=0; //Intermediate variable //中间变量 //uint8_t i=0; uint8_t check=0,check2=0, error=1,error2=1,Receive_Data_Pr[1]; //Temporary variable to save the data of the lower machine //临时变量,保存下位机数据 static int count,count2; //Static variable for counting //静态变量,用于计数 Stm32_Serial.read(Receive_Data_Pr,sizeof(Receive_Data_Pr)); //Read the data sent by the lower computer through the serial port //通过串口读取下位机发送过来的数据 /*//View the received raw data directly and debug it for use//直接查看接收到的原始数据,调试使用 ROS_INFO("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x", Receive_Data_Pr[0],Receive_Data_Pr[1],Receive_Data_Pr[2],Receive_Data_Pr[3],Receive_Data_Pr[4],Receive_Data_Pr[5],Receive_Data_Pr[6],Receive_Data_Pr[7], Receive_Data_Pr[8],Receive_Data_Pr[9],Receive_Data_Pr[10],Receive_Data_Pr[11],Receive_Data_Pr[12],Receive_Data_Pr[13],Receive_Data_Pr[14],Receive_Data_Pr[15], Receive_Data_Pr[16],Receive_Data_Pr[17],Receive_Data_Pr[18],Receive_Data_Pr[19],Receive_Data_Pr[20],Receive_Data_Pr[21],Receive_Data_Pr[22],Receive_Data_Pr[23]); */ Receive_Data.rx[count] = Receive_Data_Pr[0]; //Fill the array with serial data //串口数据填入数组 Receive_AutoCharge_Data.rx[count2] = Receive_Data_Pr[0]; 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 //接收到自动回充数据的帧头、上一个数据是24字节的帧尾,表明自动回充数据开始到来 if((Receive_Data_Pr[0] == AutoCharge_HEADER )||count2>0) count2++; else count2=0; if(Receive_Data_Pr[0] == FRAME_HEADER || count>0) //Ensure that the first data in the array is FRAME_HEADER //确保数组第一个数据为FRAME_HEADER count++; else count=0; //自动回充数据处理 if(count2 == AutoCharge_DATA_SIZE) { count2=0; if(Receive_AutoCharge_Data.rx[AutoCharge_DATA_SIZE-1]==AutoCharge_TAIL) //确认帧尾 { check2 = Check_Sum_AutoCharge(6,0);//校验位计算 if(check2 == Receive_AutoCharge_Data.rx[AutoCharge_DATA_SIZE-2]) //校验正确 { error2=0; } if(error2 == 0) //校验正确开始赋值 { transition_16 = 0; transition_16 |= Receive_AutoCharge_Data.rx[1]<<8; transition_16 |= Receive_AutoCharge_Data.rx[2]; Charging_Current = transition_16/1000+(transition_16 % 1000)*0.001; //充电电流 Red = Receive_AutoCharge_Data.rx[3]; //红外接受状态 Charging = Receive_AutoCharge_Data.rx[4];//小车充电状态 charge_set_state = Receive_AutoCharge_Data.rx[5]; check_AutoCharge_data = true; //数据成功接收标志位 } } } if(count == 24) //Verify the length of the packet //验证数据包的长度 { count=0; //Prepare for the serial port data to be refill into the array //为串口数据重新填入数组做准备 if(Receive_Data.Frame_Tail == FRAME_TAIL) //Verify the frame tail of the packet //验证数据包的帧尾 { check=Check_Sum(22,READ_DATA_CHECK); //BCC check passes or two packets are interlaced //BCC校验通过或者两组数据包交错 if(check == Receive_Data.rx[22]) { error=0; //XOR bit check successful //异或位校验成功 } if(error == 0) { /*//Check receive_data.rx for debugging use //查看Receive_Data.rx,调试使用 ROS_INFO("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x", Receive_Data.rx[0],Receive_Data.rx[1],Receive_Data.rx[2],Receive_Data.rx[3],Receive_Data.rx[4],Receive_Data.rx[5],Receive_Data.rx[6],Receive_Data.rx[7], Receive_Data.rx[8],Receive_Data.rx[9],Receive_Data.rx[10],Receive_Data.rx[11],Receive_Data.rx[12],Receive_Data.rx[13],Receive_Data.rx[14],Receive_Data.rx[15], Receive_Data.rx[16],Receive_Data.rx[17],Receive_Data.rx[18],Receive_Data.rx[19],Receive_Data.rx[20],Receive_Data.rx[21],Receive_Data.rx[22],Receive_Data.rx[23]); */ Receive_Data.Flag_Stop=Receive_Data.rx[1]; //set aside //预留位 Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]); //Get the speed of the moving chassis in the X direction //获取运动底盘X方向速度 //Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]); //Get the speed of the moving chassis in the Y direction, The Y speed is only valid in the omnidirectional mobile robot chassis //获取运动底盘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方向速度 //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.imu_rol = IMU_Trans(Receive_Data.rx[4],Receive_Data.rx[5]) * 0.01 * 3.14159/180; Mpu6050_Data.imu_yaw = IMU_Trans(Receive_Data.rx[6],Receive_Data.rx[7]) * 0.01 * 3.14159/180; 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 * 0.01 * 9.8; Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data * 0.01 * 9.8; Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data * 0.01 * 9.8; //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 * 0.01 *3.14159/180; Mpu6050.angular_velocity.y = Mpu6050_Data.gyros_y_data * 0.01 *3.14159/180; Mpu6050.angular_velocity.z = Mpu6050_Data.gyros_z_data * 0.01 *3.14159/180; //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; } /************************************** Date: January 28, 2021 Function: Loop access to the lower computer data and issue topics 功能: 循环获取下位机数据与发布话题 ***************************************/ void turn_on_robot::Control() { //_Last_Time = ros::Time::now(); _Last_Time = rclcpp::Node::now(); while(rclcpp::ok()) { try { //_Now = ros::Time::now(); _Now = rclcpp::Node::now(); Sampling_Time = (_Now - _Last_Time).seconds(); //Retrieves time interval, which is used to integrate velocity to obtain displacement (mileage) //获取时间间隔,用于积分速度获得位移(里程) if (true == Get_Sensor_Data_New()) //The serial port reads and verifies the data sent by the lower computer, and then the data is converted to international units //通过串口读取并校验下位机发送过来的数据,然后数据转换为国际单位 { //Robot_Pos.X+=(Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time; //Calculate the displacement in the X direction, unit: m //计算X方向的位移,单位:m //Robot_Pos.Y+=(Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time; //Calculate the displacement in the Y direction, unit: m //计算Y方向的位移,单位:m //Robot_Pos.Z+=Robot_Vel.Z * Sampling_Time; //The angular displacement about the Z axis, in rad //绕Z轴的角位移,单位:rad Robot_Pos.X+=(Robot_Vel.X * cos(Mpu6050_Data.imu_yaw)) * Sampling_Time; //Calculate the displacement in the X direction, unit: m //计算X方向的位移,单位:m Robot_Pos.Y+=(Robot_Vel.X * sin(Mpu6050_Data.imu_yaw)) * Sampling_Time; //Calculate the displacement in the Y direction, unit: m //计算Y方向的位移,单位:m //Robot_Pos.Z = 0 ; //Calculate the three-axis attitude from the IMU with the angular velocity around the three-axis and the three-axis acceleration //通过IMU绕三轴角速度与三轴加速度计算三轴姿态 Quaternion_Solution(Mpu6050.angular_velocity.x, Mpu6050.angular_velocity.y, Mpu6050.angular_velocity.z,\ Mpu6050.linear_acceleration.x, Mpu6050.linear_acceleration.y, Mpu6050.linear_acceleration.z); Publish_Odom(); //Pub the speedometer topic //发布里程计话题 Publish_ImuSensor(); //Pub the IMU topic //发布IMU话题 Publish_Voltage(); //Pub the topic of power supply voltage //发布电源电压话题 _Last_Time = _Now; //Record the time and use it to calculate the time interval //记录时间,用于计算时间间隔 } //自动回充数据话题 if(check_AutoCharge_data) { Publish_Charging(); //Pub a topic about whether the robot is charging //发布机器人是否在充电的话题 Publish_RED(); //Pub the topic whether the robot finds the infrared signal (charging station) //发布机器人是否寻找到红外信号(充电桩)的话题 Publish_ChargingCurrent(); //Pub the charging current topic //发布充电电流话题 check_AutoCharge_data = false; } rclcpp::spin_some(this->get_node_base_interface()); //The loop waits for the callback function //循环等待回调函数 } catch (const rclcpp::exceptions::RCLError & e ) { RCLCPP_ERROR(this->get_logger(),"unexpectedly failed whith %s",e.what()); } } } /************************************** Date: January 28, 2021 Function: Constructor, executed only once, for initialization 功能: 构造函数, 只执行一次,用于初始化 ***************************************/ turn_on_robot::turn_on_robot():rclcpp::Node ("wheeltec_robot") { Sampling_Time=0; Power_voltage=0; //Clear the data //清空数据 memset(&Robot_Pos, 0, sizeof(Robot_Pos)); memset(&Robot_Vel, 0, sizeof(Robot_Vel)); memset(&Receive_Data, 0, sizeof(Receive_Data)); memset(&Send_Data, 0, sizeof(Send_Data)); memset(&Mpu6050_Data, 0, sizeof(Mpu6050_Data)); //ros::NodeHandle private_nh("~"); //Create a node handle //创建节点句柄 //The private_nh.param() entry parameter corresponds to the initial value of the name of the parameter variable on the parameter server //private_nh.param()入口参数分别对应:参数服务器上的名称 参数变量名 初始值 this->declare_parameter<int>("serial_baud_rate",115200); this->declare_parameter<std::string>("usart_port_name", "/dev/ttyACM0"); this->declare_parameter<std::string>("odom_frame_id", "odom_combined"); this->declare_parameter<std::string>("robot_frame_id", "base_footprint"); this->declare_parameter<std::string>("gyro_frame_id", "gyro_link"); this->get_parameter("serial_baud_rate", serial_baud_rate);//Communicate baud rate 115200 to the lower machine //和下位机通信波特率115200 this->get_parameter("usart_port_name", usart_port_name);//Fixed serial port number //固定串口号 this->get_parameter("odom_frame_id", odom_frame_id);//The odometer topic corresponds to the parent TF coordinate //里程计话题对应父TF坐标 this->get_parameter("robot_frame_id", robot_frame_id);//The odometer topic corresponds to sub-TF coordinates //里程计话题对应子TF坐标 this->get_parameter("gyro_frame_id", gyro_frame_id);//IMU topics correspond to TF coordinates //IMU话题对应TF坐标 odom_publisher = create_publisher<nav_msgs::msg::Odometry>("odom", 2);//Create the odometer topic publisher //创建里程计话题发布者 imu_publisher = create_publisher<sensor_msgs::msg::Imu>("imu/data_raw", 2); //Create an IMU topic publisher //创建IMU话题发布者 voltage_publisher = create_publisher<std_msgs::msg::Float32>("PowerVoltage", 1);//Create a battery-voltage topic publisher //创建电池电压话题发布者 //回充发布者 Charging_publisher = create_publisher<std_msgs::msg::Bool>("robot_charging_flag", 10); Charging_current_publisher = create_publisher<std_msgs::msg::Float32>("robot_charging_current", 10); RED_publisher = create_publisher<std_msgs::msg::UInt8>("robot_red_flag", 10); //回充订阅者 Red_Vel_Sub = create_subscription<geometry_msgs::msg::Twist>( "red_vel", 10, std::bind(&turn_on_robot::Red_Vel_Callback, this, std::placeholders::_1)); Recharge_Flag_Sub = create_subscription<std_msgs::msg::Int8>( "robot_recharge_flag", 10, std::bind(&turn_on_robot::Recharge_Flag_Callback, this,std::placeholders::_1)); //回充服务提供 SetCharge_Service=this->create_service<turtlesim::srv::Spawn>\ ("/set_charge",std::bind(&turn_on_robot::Set_Charge_Callback,this, std::placeholders::_1 ,std::placeholders::_2)); //Set the velocity control command callback function //速度控制命令订阅回调函数设置 Cmd_Vel_Sub = create_subscription<geometry_msgs::msg::Twist>( "cmd_vel", 2, std::bind(&turn_on_robot::Cmd_Vel_Callback, this, _1)); RCLCPP_INFO(this->get_logger(),"wheeltec_robot Data ready"); //Prompt message //提示信息 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(),"wheeltec_robot can not open serial port,Please check the serial port cable! "); //If opening the serial port fails, an error message is printed //如果开启串口失败,打印错误信息 } if(Stm32_Serial.isOpen()) { RCLCPP_INFO(this->get_logger(),"wheeltec_robot serial port opened"); //Serial port opened successfully //串口开启成功提示 } } /************************************** Date: January 28, 2021 Function: Destructor, executed only once and called by the system when an object ends its life cycle 功能: 析构函数,只执行一次,当对象结束其生命周期时系统会调用这个函数 ***************************************/ turn_on_robot::~turn_on_robot() { //Sends the stop motion command to the lower machine before the turn_on_robot object ends //对象turn_on_robot结束前向下位机发送停止运动命令 Send_Data.tx[0]=0x06; Send_Data.tx[1] = FRAME_HEADER; Send_Data.tx[2] = 0; //The target velocity of the X-axis of the robot //机器人X轴的目标线速度 Send_Data.tx[4] = 0; Send_Data.tx[3] = 0; //The target velocity of the Y-axis of the robot //机器人Y轴的目标线速度 Send_Data.tx[6] = 0; Send_Data.tx[5] = 0; //The target velocity of the Z-axis of the robot //机器人Z轴的目标角速度 Send_Data.tx[8] = 0; Send_Data.tx[7] = 0; Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //Check the bits for the Check_Sum function //校验位,规则参见Check_Sum函数 Send_Data.tx[10]=0x0d; Send_Data.tx[11]=0x0a; try { Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //Send data to the serial port //向串口发数据 } catch (serial::IOException& e) { RCLCPP_ERROR(this->get_logger(),"Unable to send data through serial port"); //If sending data fails, an error message is printed //如果发送数据失败,打印错误信息 } Stm32_Serial.close(); //Close the serial port //关闭串口 RCLCPP_INFO(this->get_logger(),"Shutting down"); //Prompt message //提示信息 } turn_on_wheeltec_robot.launch.py文件代码为: import os from pathlib import Path import launch from launch.actions import SetEnvironmentVariable from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import (DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, SetEnvironmentVariable) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import PushRosNamespace import launch_ros.actions from launch.conditions import IfCondition from launch.conditions import UnlessCondition def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('turn_on_wheeltec_robot') launch_dir = os.path.join(bringup_dir, 'launch') ekf_config = Path(get_package_share_directory('turn_on_wheeltec_robot'), 'config', 'ekf.yaml') ekf_carto_config = Path(get_package_share_directory('turn_on_wheeltec_robot'), 'config', 'ekf_carto.yaml') imu_config = Path(get_package_share_directory('turn_on_wheeltec_robot'), 'config', 'imu.yaml') carto_slam = LaunchConfiguration('carto_slam', default='false') carto_slam_dec = DeclareLaunchArgument('carto_slam',default_value='false') wheeltec_robot = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'base_serial.launch.py')), ) robot_ekf = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'wheeltec_ekf.launch.py')), launch_arguments={'carto_slam':carto_slam}.items(), ) base_to_link = launch_ros.actions.Node( package='tf2_ros', executable='static_transform_publisher', name='base_to_link', arguments=['0', '0', '0','0', '0','0','base_footprint','base_link'], ) base_to_gyro = launch_ros.actions.Node( package='tf2_ros', executable='static_transform_publisher', name='base_to_gyro', arguments=['0', '0', '0','0', '0','0','base_link','gyro_link'], ) imu_filter_node = launch_ros.actions.Node( package='imu_filter_madgwick', executable='imu_filter_madgwick_node', parameters=[imu_config] ) # joint_state_publisher_node = launch_ros.actions.Node( # package='joint_state_publisher', # executable='joint_state_publisher', # name='joint_state_publisher', # ) #select a robot model,the default model is mini_mec #minibot.launch.py contains commonly used robot models #launch_arguments choices:mini_mec/mini_akm/mini_tank/mini_4wd/mini_diff/mini_omni/brushless_senior_diff #!!!At the same time, you need to modify ld.add_action(minibot_type) and #ld.add_action(flagship_type) minibot_type = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'robot_mode_description_minibot.launch.py')), launch_arguments={'bike_robot': 'true'}.items(), ) #robot_mode_description.launch.py contains flagship products, usually larger chassis robots #launch_arguments choices: #senior_akm/top_akm_bs/top_akm_dl #senior_mec_bs/senior_mec_dl/top_mec_bs/top_mec_dl/ mec_EightDrive_robot/flagship_mec_bs_robot/flagship_mec_dl_robot�� #senior_omni/top_omni #senior_4wd_bs_robot/senior_4wd_dl_robot/flagship_4wd_bs_robot/flagship_4wd_dl_robot/top_4wd_bs_robot/top_4wd_dl_robot #senior_diff_robot/four_wheel_diff_bs/four_wheel_diff_dl/flagship_four_wheel_diff_bs_robot/flagship_four_wheel_diff_dl_robot #!!!At the same time, you need to modify ld.add_action(flagship_type) and #ld.add_action(minibot_type) flagship_type = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'robot_mode_description.launch.py')), launch_arguments={'senior_akm': 'true'}.items(), ) ld = LaunchDescription() ld.add_action(minibot_type) #ld.add_action(flagship_type) ld.add_action(carto_slam_dec) ld.add_action(wheeltec_robot) ld.add_action(base_to_link) ld.add_action(base_to_gyro) # ld.add_action(joint_state_publisher_node) ld.add_action(imu_filter_node) ld.add_action(robot_ekf) return ld base_serial.launch.py文件代码为: from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition from launch.conditions import UnlessCondition import launch_ros.actions #def launch(launch_descriptor, argv): def generate_launch_description(): return LaunchDescription([ launch_ros.actions.Node( package='turn_on_wheeltec_robot', executable='wheeltec_robot_node', output='screen', parameters=[{'usart_port_name': '/dev/ttyACM0', 'serial_baud_rate':115200, 'robot_frame_id': 'base_footprint', 'odom_frame_id': 'odom_combined', 'cmd_vel': 'cmd_vel',}], ) ]) robot_mode_description_minibot.launch.py文件代码为: import os from ament_index_python.packages import get_package_share_directory import launch_ros.actions from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import LoadComposableNodes from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode from nav2_common.launch import RewrittenYaml def generate_robot_node(robot_urdf,child): return launch_ros.actions.Node( package='robot_state_publisher', executable='robot_state_publisher', name=f'robot_state_publisher_{child}', arguments=[os.path.join(get_package_share_directory('wheeltec_robot_urdf'), 'urdf', robot_urdf)], ) def generate_static_transform_publisher_node(translation, rotation, parent, child): return launch_ros.actions.Node( package='tf2_ros', executable='static_transform_publisher', name=f'base_to_{child}', arguments=[translation[0], translation[1], translation[2], rotation[0], rotation[1], rotation[2], parent, child], ) def generate_launch_description(): bike_robot = LaunchConfiguration('bike_robot', default='false') bike_robot_ = GroupAction( condition=IfCondition(bike_robot), actions=[ generate_robot_node('xuan_bike_robot.urdf','bike_robot'), generate_static_transform_publisher_node(['0.048', '0', '0.18'], ['0', '0', '0'], 'base_link', 'laser'), generate_static_transform_publisher_node(['0.048', '0', '0.18'], ['0', '0', '0'], 'base_link', 'camera_link'), ]) # Create the launch description and populate ld = LaunchDescription() # Set environment variables # Declare the launch options #ld.add_action(declare_use_composition_cmd) # Add the actions to launch all of the localiztion nodes ld.add_action(mini_mec_) ld.add_action(mini_akm_) ld.add_action(mini_tank_) ld.add_action(mini_4wd_) ld.add_action(mini_diff_) ld.add_action(brushless_senior_diff_) ld.add_action(bike_robot_) return ld xuan_bike_robot.urdf文件代码为: <?xml version="1.0"?> <robot name="wheeltec_robot"> <link name="base_link"> <visual> <geometry> <mesh filename="file:///home/deepbot/ros-relate/wheeltec_ros2/src/wheeltec_robot_urdf/wheeltec_robot_urdf/meshes/Xuan-release.stl"scale="0.001 0.001 0.001"/> </geometry> <origin xyz="0 0 0" rpy="0 0 0"/> <material name="blue"> <color rgba="0 0 0.8 0.5"/> </material> </visual> </link> </robot>
05-14
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

zhangrelay

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值