├── .gitattributes ├── CMakeLists.txt ├── README.md ├── include └── ros2_stm32_bridge │ └── ros2_stm32_driver_node.hpp ├── launch └── driver.launch.py ├── package.xml ├── params ├── stm32_2w.yaml └── stm32_4w.yaml ├── scripts ├── create_udev_rules.sh ├── delete_udev_rules.sh └── mycar.rules └── src └── base_controller.cpp /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(ros2_stm32_bridge) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(std_msgs REQUIRED) 12 | find_package(sensor_msgs REQUIRED) 13 | find_package(nav_msgs REQUIRED) 14 | find_package(geometry_msgs REQUIRED) 15 | find_package(tf2 REQUIRED) 16 | find_package(tf2_ros REQUIRED) 17 | 18 | add_executable(base_controller src/base_controller.cpp) 19 | target_include_directories(base_controller PUBLIC 20 | $ 21 | $) 22 | 23 | target_compile_features(base_controller PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 24 | ament_target_dependencies( 25 | base_controller 26 | "rclcpp" 27 | "std_msgs" 28 | "sensor_msgs" 29 | "nav_msgs" 30 | "geometry_msgs" 31 | "tf2" 32 | "tf2_ros" 33 | ) 34 | 35 | install(TARGETS base_controller 36 | DESTINATION lib/${PROJECT_NAME}) 37 | 38 | install(DIRECTORY params launch DESTINATION share/${PROJECT_NAME}) 39 | 40 | if(BUILD_TESTING) 41 | find_package(ament_lint_auto REQUIRED) 42 | # the following line skips the linter which checks for copyrights 43 | # comment the line when a copyright and license is added to all source files 44 | set(ament_cmake_copyright_FOUND TRUE) 45 | # the following line skips cpplint (only works in a git repo) 46 | # comment the line when this package is in a git repo and when 47 | # a copyright and license is added to all source files 48 | set(ament_cmake_cpplint_FOUND TRUE) 49 | ament_lint_auto_find_test_dependencies() 50 | endif() 51 | 52 | ament_package() 53 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ros2_stm32_bridge 2 | 3 | 适配以stm32为主控的两轮或四轮差速底盘的ROS2驱动。 4 | 5 | ### 使用准备 6 | 7 | #### 1.硬件准备 8 | 9 | 通过USB数据线将机器人底盘连接到上位机，并打开底盘开关。上位机终端下执行指令： 10 | 11 | ``` 12 | ll /dev/ttyUSB* 13 | ``` 14 | 15 | 如正常，将输出类似如下的结果： 16 | 17 | ``` 18 | crw-rw---- 1 root dialout 166, 0 4月 27 20:15 /dev/ttyUSB0 19 | ``` 20 | 21 | PS：如果物理连接无异常，Ubuntu系统显示`无法访问 ‘/dev/ttyUSB‘: 没有那个文件或目录`，那么可能是brltty驱动占用导致的，可以运行`sudo apt remove brltty `然后重新插拔一下设备即可解决问题。 22 | 23 | #### 2.软件安装 24 | 25 | 将此软件包下载到你的ROS2工作空间下的src目录。 26 | 27 | #### 3.环境配置 28 | 29 | ##### 3.1端口映射 30 | 31 | 终端下进入`ros2_stm32_bridge/scripts`,并执行指令： 32 | 33 | ``` 34 | bash create_udev_rules.sh 35 | ``` 36 | 37 | 该指令将为STM32端口绑定一个固定名称，重新将底盘与上位机连接，执行如下指令： 38 | 39 | ``` 40 | ll /dev | grep -i ttyUSB 41 | ``` 42 | 43 | 如正常，将输出类似如下的结果： 44 | 45 | ``` 46 | lrwxrwxrwx 1 root root 7 4月 27 20:16 mycar -> ttyUSB0 47 | crwxrwxrwx 1 root dialout 166, 0 4月 27 20:16 ttyUSB0 48 | ``` 49 | 50 | ##### 3.2设置车辆类型 51 | 52 | 修改上位机用户目录下的 .bashrc 文件，在该文件中设置车辆类型，如果你使用的是两轮差速底盘，则添加如下语句： 53 | 54 | ``` 55 | export MYCAR_MODEL=stm32_2w 56 | ``` 57 | 58 | 如果你使用的是四轮差速底盘，则添加如下语句： 59 | 60 | ``` 61 | export MYCAR_MODEL=stm32_4w 62 | ``` 63 | 64 | #### 4.构建功能包 65 | 66 | 工作空间下调用如下指令，构建功能包： 67 | 68 | ``` 69 | colcon build --packages-select ros2_stm32_bridge 70 | ``` 71 | 72 | ### 使用流程 73 | 74 | #### 1.配置参数 75 | 76 | 在功能包下提供了机器人底盘相关参数的配置文件`ros2_stm32_bridge/params`，`stm32_2w.yaml`为两轮差速底盘的配置文件,`stm32_4w.yaml`为四轮差速底盘的配置文件，参数内容可以自行修改（如果是初次使用，建议使用默认）。 77 | 78 | `stm32_2w.yaml`的文件内容如下： 79 | 80 | ``` 81 | /mini_driver: 82 | ros__parameters: 83 | base_frame: base_footprint 84 | baud_rate: 115200 85 | control_rate: 10 86 | encoder_resolution: 44.0 87 | kd: 30 88 | ki: 0 89 | kp: 25 90 | maximum_encoding: 100.0 91 | model_param_acw: 0.21 92 | model_param_cw: 0.21 93 | odom_frame: odom 94 | port_name: /dev/mycar 95 | qos_overrides: 96 | /parameter_events: 97 | publisher: 98 | depth: 1000 99 | durability: volatile 100 | history: keep_last 101 | reliability: reliable 102 | /tf: 103 | publisher: 104 | depth: 100 105 | durability: volatile 106 | history: keep_last 107 | reliability: reliable 108 | reduction_ratio: 90.0 109 | use_sim_time: false 110 | wheel_diameter: 0.065 111 | 112 | ``` 113 | 114 | `stm32_4w.yaml`的文件内容如下： 115 | 116 | ``` 117 | /mini_driver: 118 | ros__parameters: 119 | base_frame: base_footprint 120 | baud_rate: 115200 121 | control_rate: 10 122 | encoder_resolution: 44.0 123 | kd: 30 124 | ki: 0 125 | kp: 25 126 | maximum_encoding: 100.0 127 | model_param_acw: 0.45 128 | model_param_cw: 0.45 129 | odom_frame: odom 130 | port_name: /dev/mycar 131 | qos_overrides: 132 | /parameter_events: 133 | publisher: 134 | depth: 1000 135 | durability: volatile 136 | history: keep_last 137 | reliability: reliable 138 | /tf: 139 | publisher: 140 | depth: 100 141 | durability: volatile 142 | history: keep_last 143 | reliability: reliable 144 | reduction_ratio: 90.0 145 | use_sim_time: false 146 | wheel_diameter: 0.080 147 | ``` 148 | 149 | #### 2.启动launch文件 150 | 151 | 工作空间下，调用如下指令启动launch文件： 152 | 153 | ``` 154 | . install/setup.bash 155 | ros2 launch ros2_stm32_bridge driver.launch.py 156 | ``` 157 | 158 | #### 3.控制底盘运动 159 | 160 | 上位机上，启动键盘控制节点： 161 | 162 | ``` 163 | ros2 run teleop_twist_keyboard teleop_twist_keyboard 164 | ``` 165 | 166 | 接下来，就可以通过键盘控制机器人运动了。 167 | -------------------------------------------------------------------------------- /include/ros2_stm32_bridge/ros2_stm32_driver_node.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | typedef boost::shared_ptr serial_port_ptr; 20 | 21 | namespace ros2_stm32 { 22 | namespace driver { 23 | 24 | enum ParseStatus { HEADER, TYPE, DATA, CHECKSUM, END }; 25 | 26 | enum MessageType { VOLTAGE = 1, WHEEL_ENCODE = 2, ANGULAR_VELOCITY = 3, ACCELERATION = 4, ORIENTATION = 5, SPEED = 6, PID = 7, SERVO = 8 }; 27 | 28 | class MiniDriver : public rclcpp::Node { 29 | public: 30 | MiniDriver(); 31 | ~MiniDriver(); 32 | 33 | void Run(); 34 | 35 | private: 36 | void Pid(); 37 | bool Init(); 38 | void ParseMessage(); 39 | void DistributeMessage(MessageType type, uint8_t* payload); 40 | void CheckAndPublishImu(); 41 | int CalculateDelta(int ¤t_encode, int receive_encode); 42 | void HandleEncodeMessage(short left_encode, short right_encode); 43 | void TwistHandleCallback(const geometry_msgs::msg::Twist & msg); 44 | void SendTimerCallback(); 45 | void SpeedCommand(short left,short right); 46 | 47 | std::string PrintHex(uint8_t* data, int length) { 48 | std::stringstream ss; 49 | for (int i = 0; i < length; i++) { 50 | ss << std::setw(2) << std::setfill('0') << std::hex << (int)data[i] << " "; 51 | } 52 | return ss.str(); 53 | } 54 | 55 | private: 56 | bool parse_flag_; 57 | 58 | boost::system::error_code ec_; 59 | boost::asio::io_service io_service_; 60 | serial_port_ptr port_; 61 | 62 | std::mutex mutex_; 63 | std::mutex twist_mutex_; 64 | 65 | std::string port_name_; 66 | int baud_rate_; 67 | 68 | std::unique_ptr tf_broadcaster_; 69 | 70 | rclcpp::Publisher::SharedPtr voltage_publisher_; 71 | rclcpp::Publisher::SharedPtr wheel_encode_publisher_; 72 | rclcpp::Publisher::SharedPtr imu_publisher_; 73 | rclcpp::Publisher::SharedPtr odom_publisher_; 74 | rclcpp::Subscription::SharedPtr twist_subscribe_; 75 | rclcpp::TimerBase::SharedPtr send_timer_; 76 | 77 | 78 | sensor_msgs::msg::Imu imu_msg_; 79 | bool imu_msg_flag_[3]{false, false, false}; // [ANGULAR_VELOCITY, ACCELERATION, ORIENTATION] 80 | 81 | int control_rate_; 82 | std::string odom_frame_, base_frame_; 83 | 84 | double maximum_encoding_; 85 | double pulse_per_cycle_, encoder_resolution_, reduction_ratio_, pid_rate_; 86 | double model_param_cw_, model_param_acw_, wheel_diameter_; 87 | 88 | bool start_flag_; 89 | double delta_time_; 90 | double accumulation_x_, accumulation_y_, accumulation_th_; 91 | int current_left_encode_, current_right_encode_; 92 | 93 | rclcpp::Time now_, last_time_, last_twist_time_; 94 | geometry_msgs::msg::Twist current_twist_; 95 | 96 | }; 97 | 98 | } // namespace driver 99 | } // namespace ros2_stm32 100 | -------------------------------------------------------------------------------- /launch/driver.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from ament_index_python.packages import get_package_share_directory 3 | import launch_ros.actions 4 | import os 5 | 6 | def generate_launch_description(): 7 | MYCAR_MODEL = os.environ['MYCAR_MODEL'] 8 | return LaunchDescription([ 9 | launch_ros.actions.Node( 10 | package="ros2_stm32_bridge", 11 | executable="base_controller", 12 | parameters=[os.path.join(get_package_share_directory("ros2_stm32_bridge"), "params", MYCAR_MODEL + ".yaml")], 13 | ) 14 | ]) 15 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ros2_stm32_bridge 5 | 0.0.0 6 | TODO: Package description 7 | ros2 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | std_msgs 14 | sensor_msgs 15 | nav_msgs 16 | geometry_msgs 17 | tf2 18 | tf2_ros 19 | 20 | ament_lint_auto 21 | ament_lint_common 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | -------------------------------------------------------------------------------- /params/stm32_2w.yaml: -------------------------------------------------------------------------------- 1 | /mini_driver: 2 | ros__parameters: 3 | base_frame: base_footprint 4 | baud_rate: 115200 5 | control_rate: 10 6 | encoder_resolution: 44.0 7 | kd: 30 8 | ki: 0 9 | kp: 25 10 | maximum_encoding: 100.0 11 | model_param_acw: 0.21 12 | model_param_cw: 0.21 13 | odom_frame: odom 14 | port_name: /dev/mycar 15 | qos_overrides: 16 | /parameter_events: 17 | publisher: 18 | depth: 1000 19 | durability: volatile 20 | history: keep_last 21 | reliability: reliable 22 | /tf: 23 | publisher: 24 | depth: 100 25 | durability: volatile 26 | history: keep_last 27 | reliability: reliable 28 | reduction_ratio: 90.0 29 | use_sim_time: false 30 | wheel_diameter: 0.065 31 | -------------------------------------------------------------------------------- /params/stm32_4w.yaml: -------------------------------------------------------------------------------- 1 | /mini_driver: 2 | ros__parameters: 3 | base_frame: base_footprint 4 | baud_rate: 115200 5 | control_rate: 10 6 | encoder_resolution: 44.0 7 | kd: 130 8 | ki: 0 9 | kp: 100 10 | maximum_encoding: 100.0 11 | model_param_acw: 0.45 12 | model_param_cw: 0.45 13 | odom_frame: odom 14 | port_name: /dev/mycar 15 | qos_overrides: 16 | /parameter_events: 17 | publisher: 18 | depth: 1000 19 | durability: volatile 20 | history: keep_last 21 | reliability: reliable 22 | /tf: 23 | publisher: 24 | depth: 100 25 | durability: volatile 26 | history: keep_last 27 | reliability: reliable 28 | reduction_ratio: 90.0 29 | use_sim_time: false 30 | wheel_diameter: 0.080 31 | -------------------------------------------------------------------------------- /scripts/create_udev_rules.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | sudo cp mycar.rules /etc/udev/rules.d 3 | sudo service udev reload 4 | sudo service udev restart 5 | -------------------------------------------------------------------------------- /scripts/delete_udev_rules.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | sudo rm /etc/udev/rules.d/mycar.rules 3 | sudo service udev reload 4 | sudo service udev restart 5 | -------------------------------------------------------------------------------- /scripts/mycar.rules: -------------------------------------------------------------------------------- 1 | KERNEL=="ttyUSB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", MODE:="0777", SYMLINK+="mycar" 2 | 3 | -------------------------------------------------------------------------------- /src/base_controller.cpp: -------------------------------------------------------------------------------- 1 | #include "ros2_stm32_bridge/ros2_stm32_driver_node.hpp" 2 | 3 | #include "iostream" 4 | 5 | namespace ros2_stm32 { 6 | namespace driver { 7 | 8 | MiniDriver::MiniDriver() : Node("mini_driver"), parse_flag_(false), start_flag_(true) { 9 | 10 | rcl_interfaces::msg::ParameterDescriptor desc; 11 | desc.read_only = true; 12 | 13 | this->declare_parameter("port_name", "/dev/ttyUSB0",desc); 14 | this->declare_parameter("baud_rate", 115200,desc); 15 | this->declare_parameter("odom_frame", "odom",desc); 16 | this->declare_parameter("base_frame", "base_link",desc); 17 | this->declare_parameter("control_rate", 10,desc); 18 | this->declare_parameter("maximum_encoding", 100.0); 19 | this->declare_parameter("encoder_resolution", 44.0,desc); 20 | this->declare_parameter("reduction_ratio", 90.0,desc); 21 | this->declare_parameter("model_param_cw", 0.216); 22 | this->declare_parameter("model_param_acw", 0.216); 23 | this->declare_parameter("wheel_diameter", 0.065); 24 | this->declare_parameter("kp", 25); 25 | this->declare_parameter("ki", 0); 26 | this->declare_parameter("kd", 30); 27 | 28 | this->get_parameter("port_name", port_name_); 29 | this->get_parameter("baud_rate", baud_rate_); 30 | this->get_parameter("odom_frame", odom_frame_); 31 | this->get_parameter("base_frame", base_frame_); 32 | this->get_parameter("control_rate", control_rate_); 33 | // this->get_parameter("maximum_encoding", maximum_encoding_); 34 | this->get_parameter("encoder_resolution", encoder_resolution_); 35 | this->get_parameter("reduction_ratio", reduction_ratio_); 36 | // this->get_parameter("model_param_cw", model_param_cw_); 37 | // this->get_parameter("model_param_acw", model_param_acw_); 38 | // this->get_parameter("wheel_diameter", wheel_diameter_); 39 | // pulse_per_cycle_ = reduction_ratio_ * encoder_resolution_ / (M_PI * wheel_diameter_ * pid_rate_); 40 | pid_rate_ = 25.0; 41 | last_twist_time_ = this->get_clock()->now(); 42 | 43 | voltage_publisher_ = this->create_publisher("voltage", 10); 44 | wheel_encode_publisher_ = this->create_publisher("wheel_encode", 10); 45 | imu_publisher_ = this->create_publisher("imu", 10); 46 | odom_publisher_ = this->create_publisher("odom", 100); 47 | 48 | twist_subscribe_ = this->create_subscription( 49 | "cmd_vel", 10, std::bind(&MiniDriver::TwistHandleCallback, this, std::placeholders::_1)); 50 | 51 | send_timer_ = this->create_wall_timer(std::chrono::milliseconds((int)(1000 / control_rate_)), 52 | std::bind(&MiniDriver::SendTimerCallback, this)); 53 | tf_broadcaster_ = std::make_unique(*this); 54 | Run(); 55 | } 56 | 57 | MiniDriver::~MiniDriver() { 58 | mutex_.lock(); 59 | parse_flag_ = false; 60 | SpeedCommand(0,0); 61 | if (port_) { 62 | port_->cancel(); 63 | port_->close(); 64 | port_.reset(); 65 | } 66 | io_service_.stop(); 67 | io_service_.reset(); 68 | mutex_.unlock(); 69 | } 70 | 71 | bool MiniDriver::Init() { 72 | if (port_) { 73 | RCLCPP_ERROR(this->get_logger(), "error : port is already opened..."); 74 | return false; 75 | } 76 | 77 | port_ = serial_port_ptr(new boost::asio::serial_port(io_service_)); 78 | port_->open(port_name_, ec_); 79 | if (ec_) { 80 | RCLCPP_INFO_STREAM(this->get_logger(), "error : port_->open() failed...port_name=" 81 | << port_name_ << ", e=" << ec_.message().c_str() << "\n"); 82 | return false; 83 | } 84 | // option settings... 85 | port_->set_option(boost::asio::serial_port_base::baud_rate(baud_rate_)); 86 | port_->set_option(boost::asio::serial_port_base::character_size(8)); 87 | port_->set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one)); 88 | port_->set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none)); 89 | port_->set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none)); 90 | return true; 91 | } 92 | 93 | void MiniDriver::ParseMessage() { 94 | uint8_t check_num, payload[8], buffer_data[255]; 95 | MessageType msg_type; 96 | ParseStatus status = HEADER; 97 | parse_flag_ = true; 98 | while (rclcpp::ok() && parse_flag_) { 99 | switch (status) { 100 | case HEADER: 101 | boost::asio::read(*port_.get(), boost::asio::buffer(&buffer_data[0], 1), ec_); 102 | if (buffer_data[0] == 0xfc) { 103 | check_num = 0xfc; 104 | status = TYPE; 105 | } 106 | break; 107 | 108 | case TYPE: 109 | boost::asio::read(*port_.get(), boost::asio::buffer(&buffer_data[0], 1), ec_); 110 | if (buffer_data[0] >= 1 && buffer_data[0] <= 5) { 111 | msg_type = MessageType(buffer_data[0]); 112 | check_num ^= buffer_data[0]; 113 | status = DATA; 114 | } else { 115 | status = HEADER; 116 | } 117 | break; 118 | 119 | case DATA: 120 | boost::asio::read(*port_.get(), boost::asio::buffer(&payload[0], 8), ec_); 121 | for (int i = 0; i < 8; i++) { 122 | check_num ^= payload[i]; 123 | } 124 | status = CHECKSUM; 125 | break; 126 | 127 | case CHECKSUM: 128 | boost::asio::read(*port_.get(), boost::asio::buffer(&buffer_data[0], 1), ec_); 129 | if (buffer_data[0] == check_num) { 130 | status = END; 131 | } else { 132 | status = HEADER; 133 | } 134 | break; 135 | 136 | case END: 137 | boost::asio::read(*port_.get(), boost::asio::buffer(&buffer_data[0], 1), ec_); 138 | if (buffer_data[0] == 0xdf) { 139 | DistributeMessage(msg_type, payload); 140 | } 141 | status = HEADER; 142 | break; 143 | default: 144 | break; 145 | } 146 | } 147 | } 148 | 149 | void MiniDriver::DistributeMessage(MessageType type, uint8_t* payload) { 150 | if (type == VOLTAGE) { 151 | std_msgs::msg::UInt16 voltage_msg; 152 | voltage_msg.data = (payload[0] << 8 & 0xff00) | (payload[1] & 0x00ff); 153 | voltage_publisher_->publish(voltage_msg); 154 | } else if (type == WHEEL_ENCODE) { 155 | std_msgs::msg::UInt16MultiArray wheel_encode_msg; 156 | for (int i = 0; i < 4; i++) { 157 | wheel_encode_msg.data.push_back((payload[2 * i] << 8 & 0xff00) | (payload[2 * i + 1] & 0x00ff)); 158 | } 159 | wheel_encode_publisher_->publish(wheel_encode_msg); 160 | HandleEncodeMessage(wheel_encode_msg.data.at(0), wheel_encode_msg.data.at(1)); 161 | } else if (type == ANGULAR_VELOCITY) { 162 | imu_msg_.angular_velocity.x = (int16_t)((payload[0] << 8 & 0xff00) | (payload[1] & 0x00ff)) * 0.1; 163 | imu_msg_.angular_velocity.y = (int16_t)((payload[2] << 8 & 0xff00) | (payload[3] & 0x00ff)) * 0.1; 164 | imu_msg_.angular_velocity.z = (int16_t)((payload[4] << 8 & 0xff00) | (payload[5] & 0x00ff)) * 0.1; 165 | imu_msg_flag_[0] = true; 166 | CheckAndPublishImu(); 167 | } else if (type == ACCELERATION) { 168 | imu_msg_.linear_acceleration.x = (int16_t)((payload[0] << 8 & 0xff00) | (payload[1] & 0x00ff)) * 0.1; 169 | imu_msg_.linear_acceleration.y = (int16_t)((payload[2] << 8 & 0xff00) | (payload[3] & 0x00ff)) * 0.1; 170 | imu_msg_.linear_acceleration.z = (int16_t)((payload[4] << 8 & 0xff00) | (payload[5] & 0x00ff)) * 0.1; 171 | imu_msg_flag_[1] = true; 172 | CheckAndPublishImu(); 173 | } else if (type == ORIENTATION) { 174 | tf2::Quaternion quaternion; 175 | quaternion.setRPY((int16_t)((payload[0] << 8 & 0xff00) | (payload[1] & 0x00ff)) * 0.001744444, 176 | -(int16_t)((payload[2] << 8 & 0xff00) | (payload[3] & 0x00ff)) * 0.001744444, 177 | (int16_t)((payload[4] << 8 & 0xff00) | (payload[5] & 0x00ff)) * 0.001744444); 178 | imu_msg_.orientation.x = quaternion.getX(); 179 | imu_msg_.orientation.y = quaternion.getY(); 180 | imu_msg_.orientation.z = quaternion.getZ(); 181 | imu_msg_.orientation.w = quaternion.getW(); 182 | imu_msg_flag_[2] = true; 183 | CheckAndPublishImu(); 184 | } 185 | } 186 | 187 | void MiniDriver::CheckAndPublishImu() { 188 | if (imu_msg_flag_[0] && imu_msg_flag_[1] && imu_msg_flag_[2]) { 189 | imu_msg_.header.frame_id = "imu_link"; 190 | imu_msg_.header.stamp = this->get_clock()->now(); 191 | imu_publisher_->publish(imu_msg_); 192 | std::fill_n(std::begin(imu_msg_flag_), 3, false); 193 | } 194 | } 195 | 196 | int MiniDriver::CalculateDelta(int& current_encode, int receive_encode) { 197 | int delta; 198 | if (receive_encode > current_encode) { 199 | delta = (receive_encode - current_encode) < (current_encode - receive_encode + 65535) 200 | ? (receive_encode - current_encode) 201 | : (receive_encode - current_encode - 65535); 202 | } else { 203 | delta = (current_encode - receive_encode) < (receive_encode - current_encode + 65535) 204 | ? (receive_encode - current_encode) 205 | : (receive_encode - current_encode + 65535); 206 | } 207 | current_encode = receive_encode; 208 | return delta; 209 | } 210 | 211 | void MiniDriver::HandleEncodeMessage(short left_encode, short right_encode) { 212 | now_ = this->get_clock()->now(); 213 | if (start_flag_) { 214 | accumulation_x_ = 0.0; 215 | accumulation_y_ = 0.0; 216 | accumulation_th_ = 0.0; 217 | current_left_encode_ = left_encode; 218 | current_right_encode_ = right_encode; 219 | last_time_ = now_; 220 | start_flag_ = false; 221 | return; 222 | } 223 | int delta_left = CalculateDelta(current_left_encode_, left_encode); 224 | int delta_right = CalculateDelta(current_right_encode_, right_encode); 225 | delta_time_ = (now_ - last_time_).seconds(); 226 | if (delta_time_ > 0.02) { 227 | double model_param; 228 | this->get_parameter("model_param_cw", model_param_cw_); 229 | this->get_parameter("model_param_acw", model_param_acw_); 230 | if (delta_right <= delta_left) { 231 | model_param = model_param_cw_; 232 | } else { 233 | model_param = model_param_acw_; 234 | } 235 | this->get_parameter("wheel_diameter", wheel_diameter_); 236 | pulse_per_cycle_ = reduction_ratio_ * encoder_resolution_ / (M_PI * wheel_diameter_ * pid_rate_); 237 | double delta_theta = (delta_right - delta_left) / (pulse_per_cycle_ * pid_rate_ * model_param); 238 | double v_theta = delta_theta / delta_time_; 239 | 240 | double delta_dis = (delta_right + delta_left) / (pulse_per_cycle_ * pid_rate_ * 2.0); 241 | double v_dis = delta_dis / delta_time_; 242 | double delta_x, delta_y; 243 | if (delta_theta == 0) { 244 | delta_x = delta_dis; 245 | delta_y = 0.0; 246 | } else { 247 | delta_x = delta_dis * (sin(delta_theta) / delta_theta); 248 | delta_y = delta_dis * ((1 - cos(delta_theta)) / delta_theta); 249 | } 250 | 251 | accumulation_x_ += (cos(accumulation_th_) * delta_x - sin(accumulation_th_) * delta_y); 252 | accumulation_y_ += (sin(accumulation_th_) * delta_x + cos(accumulation_th_) * delta_y); 253 | accumulation_th_ += delta_theta; 254 | 255 | geometry_msgs::msg::TransformStamped transform_stamped; 256 | transform_stamped.header.stamp = this->get_clock()->now(); 257 | transform_stamped.header.frame_id = odom_frame_; 258 | transform_stamped.child_frame_id = base_frame_; 259 | transform_stamped.transform.translation.x = accumulation_x_; 260 | transform_stamped.transform.translation.y = accumulation_y_; 261 | transform_stamped.transform.translation.z = 0.0; 262 | tf2::Quaternion q; 263 | q.setRPY(0, 0, accumulation_th_); 264 | transform_stamped.transform.rotation.x = q.x(); 265 | transform_stamped.transform.rotation.y = q.y(); 266 | transform_stamped.transform.rotation.z = q.z(); 267 | transform_stamped.transform.rotation.w = q.w(); 268 | tf_broadcaster_->sendTransform(transform_stamped); 269 | 270 | nav_msgs::msg::Odometry odom_msg; 271 | odom_msg.header.stamp = now_; 272 | odom_msg.header.frame_id = odom_frame_; 273 | odom_msg.child_frame_id = base_frame_; 274 | odom_msg.pose.pose.position.x = accumulation_x_; 275 | odom_msg.pose.pose.position.y = accumulation_y_; 276 | odom_msg.pose.pose.position.z = 0; 277 | odom_msg.pose.pose.orientation.x = q.getX(); 278 | odom_msg.pose.pose.orientation.y = q.getY(); 279 | odom_msg.pose.pose.orientation.z = q.getZ(); 280 | odom_msg.pose.pose.orientation.w = q.getW(); 281 | odom_msg.twist.twist.linear.x = v_dis; 282 | 283 | odom_msg.twist.twist.linear.y = 0; 284 | odom_msg.twist.twist.angular.z = v_theta; 285 | odom_publisher_->publish(odom_msg); 286 | } 287 | last_time_ = now_; 288 | } 289 | 290 | void MiniDriver::TwistHandleCallback(const geometry_msgs::msg::Twist& msg) { 291 | twist_mutex_.lock(); 292 | last_twist_time_ = this->get_clock()->now(); 293 | current_twist_ = msg; 294 | Pid(); 295 | twist_mutex_.unlock(); 296 | } 297 | 298 | void MiniDriver::SpeedCommand(short left,short right){ 299 | uint8_t data[12] = {0xfc, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xdf}; 300 | data[2] = data[6] = (left >> 8) & 0xff; 301 | data[3] = data[7] = left & 0xff; 302 | data[4] = data[8] = (right >> 8) & 0xff; 303 | data[5] = data[9] = right & 0xff; 304 | for (int i = 0; i < 10; i++) { 305 | data[10] ^= data[i]; 306 | } 307 | boost::asio::write(*port_.get(), boost::asio::buffer(data, 12), ec_); 308 | } 309 | 310 | void MiniDriver::SendTimerCallback() { 311 | double left_d, right_d, radio; 312 | double model_param; 313 | short left, right; 314 | 315 | double linear_speed, angular_speed; 316 | if ((this->get_clock()->now() - last_twist_time_).seconds() <= 1.0) { 317 | linear_speed = current_twist_.linear.x; 318 | angular_speed = current_twist_.angular.z; 319 | } else { 320 | linear_speed = 0; 321 | angular_speed = 0; 322 | } 323 | if (angular_speed <= 0) { 324 | model_param = model_param_cw_; 325 | } else { 326 | model_param = model_param_acw_; 327 | } 328 | 329 | left_d = (linear_speed - model_param / 2 * angular_speed) * pulse_per_cycle_; 330 | right_d = (linear_speed + model_param / 2 * angular_speed) * pulse_per_cycle_; 331 | 332 | this->get_parameter("maximum_encoding", maximum_encoding_); 333 | radio = std::max(std::max(std::abs(left_d), std::abs(right_d)) / maximum_encoding_, 1.0); 334 | 335 | left = static_cast(left_d / radio); 336 | right = static_cast(right_d / radio); 337 | 338 | SpeedCommand(left,right); 339 | 340 | } 341 | 342 | void MiniDriver::Pid(){ 343 | int64_t kp,ki,kd; 344 | this->get_parameter("kp",kp); 345 | this->get_parameter("ki",ki); 346 | this->get_parameter("kd",kd); 347 | uint8_t data[12] = {0xfc, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xdf}; 348 | data[2] = (kp >> 8) & 0xff; 349 | data[3] = kp & 0xff; 350 | data[4] = (ki >> 8) & 0xff; 351 | data[5] = ki & 0xff; 352 | data[6] = (kd >> 8) & 0xff; 353 | data[7] = kd & 0xff; 354 | for (int i = 0; i < 10; i++) { 355 | data[10] ^= data[i]; 356 | } 357 | 358 | boost::asio::write(*port_.get(), boost::asio::buffer(data, 12), ec_); 359 | } 360 | void MiniDriver::Run() { 361 | if (Init()) { 362 | Pid(); 363 | std::thread parse_thread(&MiniDriver::ParseMessage, this); 364 | parse_thread.detach(); 365 | } 366 | } 367 | 368 | } // namespace driver 369 | } // namespace ros2_stm32 370 | 371 | int main(int argc, char** argv) { 372 | rclcpp::init(argc, argv); 373 | rclcpp::spin(std::make_shared()); 374 | rclcpp::shutdown(); 375 | return 0; 376 | } 377 | --------------------------------------------------------------------------------