写完状态机了
This commit is contained in:
		
							parent
							
								
									d0630a82c7
								
							
						
					
					
						commit
						7cd85cbbf3
					
				
							
								
								
									
										3
									
								
								.vscode/settings.json
									
									
									
									
										vendored
									
									
								
							
							
						
						
									
										3
									
								
								.vscode/settings.json
									
									
									
									
										vendored
									
									
								
							| @ -1,5 +1,6 @@ | |||||||
| { | { | ||||||
|     "files.associations": { |     "files.associations": { | ||||||
|         "chrono": "cpp" |         "chrono": "cpp", | ||||||
|  |         "functional": "cpp" | ||||||
|     } |     } | ||||||
| } | } | ||||||
| @ -7,28 +7,43 @@ endif() | |||||||
| 
 | 
 | ||||||
| # find dependencies | # find dependencies | ||||||
| find_package(ament_cmake REQUIRED) | find_package(ament_cmake REQUIRED) | ||||||
| # uncomment the following section in order to fill in | find_package(rclcpp REQUIRED) | ||||||
| # further dependencies manually. | find_package(rc_msgs REQUIRED) | ||||||
| # find_package(<dependency> REQUIRED) | find_package(control_input_msgs REQUIRED) | ||||||
|  | find_package(std_msgs REQUIRED) | ||||||
|  | find_package(geometry_msgs REQUIRED) | ||||||
|  | 
 | ||||||
|  | include_directories( | ||||||
|  |   include | ||||||
|  | ) | ||||||
|  | 
 | ||||||
|  | 
 | ||||||
|  | add_executable(mdog_simple_controller | ||||||
|  |   src/mdog_simple_controller.cpp | ||||||
|  |   src/FSM/state_zero.cpp | ||||||
|  |   src/FSM/state_balance.cpp | ||||||
|  |   src/FSM/state_troting.cpp | ||||||
|  |   src/common/kinematics.cpp | ||||||
|  | ) | ||||||
|  | ament_target_dependencies(mdog_simple_controller rclcpp rc_msgs std_msgs control_input_msgs geometry_msgs) | ||||||
|  | 
 | ||||||
|  | install(TARGETS | ||||||
|  |   mdog_simple_controller | ||||||
|  |   DESTINATION lib/${PROJECT_NAME} | ||||||
|  | ) | ||||||
| 
 | 
 | ||||||
| install( | install( | ||||||
|   DIRECTORY launch |   DIRECTORY launch | ||||||
|   DESTINATION share/${PROJECT_NAME}/ |   DESTINATION share/${PROJECT_NAME}/ | ||||||
| ) | ) | ||||||
| 
 | 
 | ||||||
| install( | install(DIRECTORY include/ | ||||||
|   DIRECTORY include/ |  | ||||||
|   DESTINATION include |   DESTINATION include | ||||||
| ) | ) | ||||||
| 
 | 
 | ||||||
| if(BUILD_TESTING) | if(BUILD_TESTING) | ||||||
|   find_package(ament_lint_auto REQUIRED) |   find_package(ament_lint_auto REQUIRED) | ||||||
|   # the following line skips the linter which checks for copyrights |  | ||||||
|   # comment the line when a copyright and license is added to all source files |  | ||||||
|   set(ament_cmake_copyright_FOUND TRUE) |   set(ament_cmake_copyright_FOUND TRUE) | ||||||
|   # the following line skips cpplint (only works in a git repo) |  | ||||||
|   # comment the line when this package is in a git repo and when |  | ||||||
|   # a copyright and license is added to all source files |  | ||||||
|   set(ament_cmake_cpplint_FOUND TRUE) |   set(ament_cmake_cpplint_FOUND TRUE) | ||||||
|   ament_lint_auto_find_test_dependencies() |   ament_lint_auto_find_test_dependencies() | ||||||
| endif() | endif() | ||||||
|  | |||||||
| @ -0,0 +1,15 @@ | |||||||
|  | #pragma once | ||||||
|  | 
 | ||||||
|  | namespace mdog_simple_controller { | ||||||
|  | 
 | ||||||
|  | class MdogSimpleController; | ||||||
|  | 
 | ||||||
|  | class FSMState { | ||||||
|  | public: | ||||||
|  |     virtual ~FSMState() = default; | ||||||
|  |     virtual void enter(MdogSimpleController*) {} | ||||||
|  |     virtual void run(MdogSimpleController*) = 0; | ||||||
|  |     virtual void exit(MdogSimpleController*) {} | ||||||
|  | }; | ||||||
|  | 
 | ||||||
|  | } // namespace mdog_simple_controller
 | ||||||
| @ -0,0 +1,13 @@ | |||||||
|  | #pragma once | ||||||
|  | #include "mdog_simple_controller/FSM/fsm_state.hpp" | ||||||
|  | 
 | ||||||
|  | namespace mdog_simple_controller { | ||||||
|  | 
 | ||||||
|  | class StateBalance : public FSMState { | ||||||
|  | public: | ||||||
|  |     void enter(MdogSimpleController*) override; | ||||||
|  |     void run(MdogSimpleController*) override; | ||||||
|  |     void exit(MdogSimpleController*) override; | ||||||
|  | }; | ||||||
|  | 
 | ||||||
|  | } | ||||||
| @ -0,0 +1,13 @@ | |||||||
|  | #pragma once | ||||||
|  | #include "mdog_simple_controller/FSM/fsm_state.hpp" | ||||||
|  | 
 | ||||||
|  | namespace mdog_simple_controller { | ||||||
|  | 
 | ||||||
|  | class StateTroting : public FSMState { | ||||||
|  | public: | ||||||
|  |     void enter(MdogSimpleController*) override; | ||||||
|  |     void run(MdogSimpleController*) override; | ||||||
|  |     void exit(MdogSimpleController*) override; | ||||||
|  | }; | ||||||
|  | 
 | ||||||
|  | } | ||||||
| @ -0,0 +1,13 @@ | |||||||
|  | #pragma once | ||||||
|  | #include "mdog_simple_controller/FSM/fsm_state.hpp" | ||||||
|  | 
 | ||||||
|  | namespace mdog_simple_controller { | ||||||
|  | 
 | ||||||
|  | class StateZero : public FSMState { | ||||||
|  | public: | ||||||
|  |     void enter(MdogSimpleController*) override; | ||||||
|  |     void run(MdogSimpleController*) override; | ||||||
|  |     void exit(MdogSimpleController*) override; | ||||||
|  | }; | ||||||
|  | 
 | ||||||
|  | } | ||||||
| @ -0,0 +1,21 @@ | |||||||
|  | #pragma once | ||||||
|  | #include <array> | ||||||
|  | 
 | ||||||
|  | namespace mdog_simple_controller { | ||||||
|  | namespace kinematics { | ||||||
|  | 
 | ||||||
|  | // 固定三连杆长度(单位:米,可根据实际机器人参数修改)
 | ||||||
|  | constexpr std::array<double, 3> LEG_LINK_LENGTH = {0.05, 0.30, 0.30}; | ||||||
|  | 
 | ||||||
|  | // 正运动学:已知关节角,求末端位置
 | ||||||
|  | void forward_kinematics(const std::array<double, 3>& theta, | ||||||
|  |                         const std::array<double, 3>& link, | ||||||
|  |                         std::array<double, 3>& pos); | ||||||
|  | 
 | ||||||
|  | // 逆运动学:已知末端位置,求关节角。返回值为是否可达
 | ||||||
|  | bool inverse_kinematics(const std::array<double, 3>& pos, | ||||||
|  |                         const std::array<double, 3>& link, | ||||||
|  |                         std::array<double, 3>& theta); | ||||||
|  | 
 | ||||||
|  | } // namespace kinematics
 | ||||||
|  | } // namespace mdog_simple_controller
 | ||||||
| @ -0,0 +1,81 @@ | |||||||
|  | #pragma once | ||||||
|  | 
 | ||||||
|  | #include <rclcpp/rclcpp.hpp> | ||||||
|  | #include "rc_msgs/msg/leg_fdb.hpp" | ||||||
|  | #include "rc_msgs/msg/leg_cmd.hpp" | ||||||
|  | #include "control_input_msgs/msg/inputs.hpp" | ||||||
|  | #include "geometry_msgs/msg/vector3.hpp" | ||||||
|  | #include "mdog_simple_controller/FSM/state_zero.hpp" | ||||||
|  | #include "mdog_simple_controller/FSM/state_balance.hpp" | ||||||
|  | #include "mdog_simple_controller/FSM/state_troting.hpp" | ||||||
|  | 
 | ||||||
|  | namespace mdog_simple_controller | ||||||
|  | { | ||||||
|  | 
 | ||||||
|  | struct LegJointData { | ||||||
|  |     float torque[3]; | ||||||
|  |     float speed[3]; | ||||||
|  |     float pos[3]; | ||||||
|  | }; | ||||||
|  | 
 | ||||||
|  | struct LegJointCmd { | ||||||
|  |     float torque_des[3]; | ||||||
|  |     float speed_des[3]; | ||||||
|  |     float pos_des[3]; | ||||||
|  |     float kp[3]; | ||||||
|  |     float kd[3]; | ||||||
|  | }; | ||||||
|  | 
 | ||||||
|  | struct Voltage{ | ||||||
|  |     float vx; | ||||||
|  |     float vy; | ||||||
|  |     float wz; | ||||||
|  | }; | ||||||
|  | 
 | ||||||
|  | struct AHRS_Eulr_t { | ||||||
|  |     float yaw; | ||||||
|  |     float rol; | ||||||
|  |     float pit; | ||||||
|  | }; | ||||||
|  | 
 | ||||||
|  | struct InputCmd { | ||||||
|  |     Voltage voltage; | ||||||
|  |     AHRS_Eulr_t imu_euler; | ||||||
|  |     AHRS_Eulr_t machine_euler; | ||||||
|  |     float hight; | ||||||
|  |     int8_t status; | ||||||
|  | }; | ||||||
|  | 
 | ||||||
|  | struct MdogControllerData { | ||||||
|  |     LegJointData feedback[4]; | ||||||
|  |     LegJointCmd command[4]; | ||||||
|  | }; | ||||||
|  | 
 | ||||||
|  | 
 | ||||||
|  | class MdogSimpleController : public rclcpp::Node { | ||||||
|  | public: | ||||||
|  |     MdogSimpleController(); | ||||||
|  | 
 | ||||||
|  |     void change_state(FSMState* new_state); | ||||||
|  |      | ||||||
|  |     MdogControllerData data_; | ||||||
|  |     InputCmd input_cmd_; | ||||||
|  | private: | ||||||
|  |     void fdb_callback(const rc_msgs::msg::LegFdb::SharedPtr msg); | ||||||
|  |     void input_callback(const control_input_msgs::msg::Inputs::SharedPtr msg); | ||||||
|  |     void ahrs_callback(const geometry_msgs::msg::Vector3::SharedPtr msg); | ||||||
|  |     void control_loop(); | ||||||
|  | 
 | ||||||
|  |     FSMState* current_state_{nullptr}; | ||||||
|  |     StateZero state_zero_; | ||||||
|  |     StateBalance state_balance_; | ||||||
|  |     StateTroting state_troting_; | ||||||
|  | 
 | ||||||
|  |     rclcpp::Subscription<rc_msgs::msg::LegFdb>::SharedPtr fdb_sub_; | ||||||
|  |     rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr input_sub_; | ||||||
|  |     rclcpp::Subscription<geometry_msgs::msg::Vector3>::SharedPtr ahrs_sub_; | ||||||
|  |     rclcpp::Publisher<rc_msgs::msg::LegCmd>::SharedPtr cmd_pub_; | ||||||
|  |     rclcpp::TimerBase::SharedPtr timer_; | ||||||
|  | }; | ||||||
|  | 
 | ||||||
|  | } // namespace mdog_simple_controller
 | ||||||
| @ -8,6 +8,11 @@ | |||||||
|   <license>TODO: License declaration</license> |   <license>TODO: License declaration</license> | ||||||
| 
 | 
 | ||||||
|   <buildtool_depend>ament_cmake</buildtool_depend> |   <buildtool_depend>ament_cmake</buildtool_depend> | ||||||
|  |   <depend>rclcpp</depend> | ||||||
|  |   <depend>rc_msgs</depend> | ||||||
|  |   <depend>control_input_msgs</depend> | ||||||
|  |   <depend>geometry_msgs</depend> | ||||||
|  |   <depend>std_msgs</depend> | ||||||
| 
 | 
 | ||||||
|   <test_depend>ament_lint_auto</test_depend> |   <test_depend>ament_lint_auto</test_depend> | ||||||
|   <test_depend>ament_lint_common</test_depend> |   <test_depend>ament_lint_common</test_depend> | ||||||
|  | |||||||
| @ -0,0 +1,18 @@ | |||||||
|  | #include "mdog_simple_controller/FSM/state_balance.hpp" | ||||||
|  | #include "mdog_simple_controller/mdog_simple_controller.hpp" | ||||||
|  | 
 | ||||||
|  | namespace mdog_simple_controller { | ||||||
|  | 
 | ||||||
|  | void StateBalance::enter(MdogSimpleController* ctrl) { | ||||||
|  |     // 进入BALANCE状态时的初始化
 | ||||||
|  | } | ||||||
|  | 
 | ||||||
|  | void StateBalance::run(MdogSimpleController* ctrl) { | ||||||
|  |     // BALANCE状态下的控制逻辑
 | ||||||
|  | } | ||||||
|  | 
 | ||||||
|  | void StateBalance::exit(MdogSimpleController* ctrl) { | ||||||
|  |     // 离开BALANCE状态时的清理
 | ||||||
|  | } | ||||||
|  | 
 | ||||||
|  | } | ||||||
| @ -0,0 +1,27 @@ | |||||||
|  | #include "mdog_simple_controller/FSM/state_troting.hpp" | ||||||
|  | #include "mdog_simple_controller/mdog_simple_controller.hpp" | ||||||
|  | 
 | ||||||
|  | namespace mdog_simple_controller { | ||||||
|  | 
 | ||||||
|  | void StateTroting::enter(MdogSimpleController* ctrl) { | ||||||
|  |     // 进入TROTING状态时的初始化
 | ||||||
|  | } | ||||||
|  | 
 | ||||||
|  | void StateTroting::run(MdogSimpleController* ctrl) { | ||||||
|  |     //设置所有cmd数据为0
 | ||||||
|  |     for (int leg = 0; leg < 4; ++leg) { | ||||||
|  |         for (int j = 0; j < 3; ++j) { | ||||||
|  |             ctrl->data_.command[leg].torque_des[j] = 0; | ||||||
|  |             ctrl->data_.command[leg].speed_des[j] = 0; | ||||||
|  |             ctrl->data_.command[leg].pos_des[j] = 0; | ||||||
|  |             ctrl->data_.command[leg].kp[j] = 0; | ||||||
|  |             ctrl->data_.command[leg].kd[j] = 0.05; | ||||||
|  |         } | ||||||
|  |     } | ||||||
|  | } | ||||||
|  | 
 | ||||||
|  | void StateTroting::exit(MdogSimpleController* ctrl) { | ||||||
|  |     // 离开TROTING状态时的清理
 | ||||||
|  | } | ||||||
|  | 
 | ||||||
|  | } | ||||||
| @ -0,0 +1,27 @@ | |||||||
|  | #include "mdog_simple_controller/FSM/state_zero.hpp" | ||||||
|  | #include "mdog_simple_controller/mdog_simple_controller.hpp" | ||||||
|  | 
 | ||||||
|  | namespace mdog_simple_controller { | ||||||
|  | 
 | ||||||
|  | void StateZero::enter(MdogSimpleController* ctrl) { | ||||||
|  |     // 清空电机命令
 | ||||||
|  |     for (int leg = 0; leg < 4; ++leg) { | ||||||
|  |         for (int j = 0; j < 3; ++j) { | ||||||
|  |             ctrl->data_.command[leg].torque_des[j] = 0; | ||||||
|  |             ctrl->data_.command[leg].speed_des[j] = 0; | ||||||
|  |             ctrl->data_.command[leg].pos_des[j] = 0; | ||||||
|  |             ctrl->data_.command[leg].kp[j] = 0; | ||||||
|  |             ctrl->data_.command[leg].kd[j] = 0; | ||||||
|  |         } | ||||||
|  |     } | ||||||
|  | } | ||||||
|  | 
 | ||||||
|  | void StateZero::run(MdogSimpleController* ctrl) { | ||||||
|  |     // ZERO状态下的控制逻辑
 | ||||||
|  | } | ||||||
|  | 
 | ||||||
|  | void StateZero::exit(MdogSimpleController* ctrl) { | ||||||
|  |     // 离开ZERO状态时的清理
 | ||||||
|  | } | ||||||
|  | 
 | ||||||
|  | } | ||||||
| @ -0,0 +1,46 @@ | |||||||
|  | #include "mdog_simple_controller/common/kinematics.hpp" | ||||||
|  | #include <cmath> | ||||||
|  | 
 | ||||||
|  | namespace mdog_simple_controller { | ||||||
|  | namespace kinematics { | ||||||
|  | 
 | ||||||
|  | void forward_kinematics(const std::array<double, 3>& theta, | ||||||
|  |                         const std::array<double, 3>& link, | ||||||
|  |                         std::array<double, 3>& pos) { | ||||||
|  |     double t1 = theta[0], t2 = theta[1], t3 = theta[2]; | ||||||
|  |     double l1 = link[0], l2 = link[1], l3 = link[2]; | ||||||
|  | 
 | ||||||
|  |     double x = (l2 * cos(t2) + l3 * cos(t2 + t3)) * cos(t1); | ||||||
|  |     double y = (l2 * cos(t2) + l3 * cos(t2 + t3)) * sin(t1); | ||||||
|  |     double z = l1 + l2 * sin(t2) + l3 * sin(t2 + t3); | ||||||
|  | 
 | ||||||
|  |     pos[0] = x; | ||||||
|  |     pos[1] = y; | ||||||
|  |     pos[2] = z; | ||||||
|  | } | ||||||
|  | 
 | ||||||
|  | bool inverse_kinematics(const std::array<double, 3>& pos, | ||||||
|  |                         const std::array<double, 3>& link, | ||||||
|  |                         std::array<double, 3>& theta) { | ||||||
|  |     double x = pos[0], y = pos[1], z = pos[2]; | ||||||
|  |     double l1 = link[0], l2 = link[1], l3 = link[2]; | ||||||
|  | 
 | ||||||
|  |     theta[0] = atan2(y, x); | ||||||
|  | 
 | ||||||
|  |     double d = sqrt(x * x + y * y); | ||||||
|  |     double z1 = z - l1; | ||||||
|  | 
 | ||||||
|  |     double D = (d * d + z1 * z1 - l2 * l2 - l3 * l3) / (2 * l2 * l3); | ||||||
|  |     if (D < -1.0 || D > 1.0) return false; // 不可达
 | ||||||
|  | 
 | ||||||
|  |     theta[2] = atan2(-sqrt(1 - D * D), D); | ||||||
|  | 
 | ||||||
|  |     double phi = atan2(z1, d); | ||||||
|  |     double psi = atan2(l3 * sin(theta[2]), l2 + l3 * cos(theta[2])); | ||||||
|  |     theta[1] = phi - psi; | ||||||
|  | 
 | ||||||
|  |     return true; | ||||||
|  | } | ||||||
|  | 
 | ||||||
|  | } // namespace kinematics
 | ||||||
|  | } // namespace mdog_simple_controller
 | ||||||
| @ -0,0 +1,108 @@ | |||||||
|  | #include "mdog_simple_controller/mdog_simple_controller.hpp" | ||||||
|  | 
 | ||||||
|  | namespace mdog_simple_controller | ||||||
|  | { | ||||||
|  | 
 | ||||||
|  | MdogSimpleController::MdogSimpleController() : Node("mdog_simple_controller") { | ||||||
|  |     fdb_sub_ = this->create_subscription<rc_msgs::msg::LegFdb>( | ||||||
|  |         "/leg_fdb", 10, | ||||||
|  |         std::bind(&MdogSimpleController::fdb_callback, this, std::placeholders::_1)); | ||||||
|  |     input_sub_ = this->create_subscription<control_input_msgs::msg::Inputs>( | ||||||
|  |         "/control_input", 10, | ||||||
|  |         std::bind(&MdogSimpleController::input_callback, this, std::placeholders::_1)); | ||||||
|  |     ahrs_sub_ = this->create_subscription<geometry_msgs::msg::Vector3>( | ||||||
|  |         "/euler_angles", 10, | ||||||
|  |         std::bind(&MdogSimpleController::ahrs_callback, this, std::placeholders::_1)); | ||||||
|  |     cmd_pub_ = this->create_publisher<rc_msgs::msg::LegCmd>("/leg_cmd", 10); | ||||||
|  |     current_state_ = &state_zero_; | ||||||
|  |     current_state_->enter(this); | ||||||
|  |     timer_ = this->create_wall_timer( | ||||||
|  |         std::chrono::microseconds(2000), | ||||||
|  |         std::bind(&MdogSimpleController::control_loop, this)); | ||||||
|  | } | ||||||
|  | 
 | ||||||
|  | void MdogSimpleController::change_state(FSMState* new_state) { | ||||||
|  |     if (current_state_) current_state_->exit(this); | ||||||
|  |     current_state_ = new_state; | ||||||
|  |     if (current_state_) current_state_->enter(this); | ||||||
|  | } | ||||||
|  | 
 | ||||||
|  | void MdogSimpleController::fdb_callback(const rc_msgs::msg::LegFdb::SharedPtr msg) { | ||||||
|  |     for (int leg = 0; leg < 4; ++leg) { | ||||||
|  |         for (int j = 0; j < 3; ++j) { | ||||||
|  |             int idx = leg * 3 + j; | ||||||
|  |             data_.feedback[leg].torque[j] = msg->leg_fdb[idx].torque; | ||||||
|  |             data_.feedback[leg].speed[j]  = msg->leg_fdb[idx].speed; | ||||||
|  |             data_.feedback[leg].pos[j]    = msg->leg_fdb[idx].pos; | ||||||
|  |         } | ||||||
|  |     } | ||||||
|  | } | ||||||
|  | 
 | ||||||
|  | void MdogSimpleController::input_callback(const control_input_msgs::msg::Inputs::SharedPtr msg){ | ||||||
|  |     input_cmd_.voltage.vx = msg->lx; | ||||||
|  |     input_cmd_.voltage.vy = msg->ly; | ||||||
|  |     input_cmd_.voltage.wz = msg->rx; | ||||||
|  |     input_cmd_.hight = 0.5; | ||||||
|  |     input_cmd_.status = msg->command; | ||||||
|  | } | ||||||
|  | 
 | ||||||
|  | void MdogSimpleController::ahrs_callback(const geometry_msgs::msg::Vector3::SharedPtr msg) { | ||||||
|  |     input_cmd_.imu_euler.yaw = msg->z; | ||||||
|  |     input_cmd_.imu_euler.rol = msg->x; | ||||||
|  |     input_cmd_.imu_euler.pit = msg->y; | ||||||
|  | } | ||||||
|  | 
 | ||||||
|  | void MdogSimpleController::control_loop() { | ||||||
|  |     FSMState* target_state = nullptr; | ||||||
|  |     switch (input_cmd_.status) { | ||||||
|  |         case 0: target_state = &state_zero_; break; | ||||||
|  |         case 6: target_state = &state_balance_; break; | ||||||
|  |         case 7: target_state = &state_troting_; break; | ||||||
|  |         default: target_state = &state_zero_; break; | ||||||
|  |     } | ||||||
|  |     if (target_state != current_state_) { | ||||||
|  |         change_state(target_state); | ||||||
|  |     } | ||||||
|  |     if (current_state_) current_state_->run(this); | ||||||
|  | 
 | ||||||
|  |     rc_msgs::msg::LegCmd msg; | ||||||
|  |     for (int leg = 0; leg < 4; ++leg) { | ||||||
|  |         for (int j = 0; j < 3; ++j) { | ||||||
|  |             msg.leg_cmd[leg * 3 + j].torque_des = data_.command[leg].torque_des[j]; | ||||||
|  |             msg.leg_cmd[leg * 3 + j].speed_des = data_.command[leg].speed_des[j]; | ||||||
|  |             msg.leg_cmd[leg * 3 + j].pos_des = data_.command[leg].pos_des[j]; | ||||||
|  |             msg.leg_cmd[leg * 3 + j].kp = data_.command[leg].kp[j]; | ||||||
|  |             msg.leg_cmd[leg * 3 + j].kd = data_.command[leg].kd[j]; | ||||||
|  |         } | ||||||
|  |     } | ||||||
|  |     msg.header.stamp = this->now(); | ||||||
|  |     msg.header.frame_id = "leg_cmd"; | ||||||
|  |     cmd_pub_->publish(msg); | ||||||
|  | 
 | ||||||
|  |     if (0){ | ||||||
|  |         RCLCPP_INFO(this->get_logger(), "MdogControllerData:"); | ||||||
|  |         for (int leg = 0; leg < 4; ++leg) { | ||||||
|  |             RCLCPP_INFO(this->get_logger(), "Leg %d:", leg); | ||||||
|  |             for (int j = 0; j < 3; ++j) { | ||||||
|  |                 RCLCPP_INFO(this->get_logger(), "Joint %d: Torque: %f, Speed: %f, Pos: %f", | ||||||
|  |                     j, data_.feedback[leg].torque[j], data_.feedback[leg].speed[j], data_.feedback[leg].pos[j]); | ||||||
|  |             } | ||||||
|  |         } | ||||||
|  |         RCLCPP_INFO(this->get_logger(), "InputCmd: Voltage: [%f, %f, %f], Height: %f, Status: %d", | ||||||
|  |             input_cmd_.voltage.vx, input_cmd_.voltage.vy, input_cmd_.voltage.wz, input_cmd_.hight, input_cmd_.status); | ||||||
|  |         RCLCPP_INFO(this->get_logger(), "AHRS: Yaw: %f, Roll: %f, Pitch: %f", | ||||||
|  |             input_cmd_.imu_euler.yaw, input_cmd_.imu_euler.rol, input_cmd_.imu_euler.pit); | ||||||
|  |     } | ||||||
|  |     RCLCPP_INFO(this->get_logger(), "InputCmd: Voltage: [%f, %f, %f], Height: %f, Status: %d", | ||||||
|  |         input_cmd_.voltage.vx, input_cmd_.voltage.vy, input_cmd_.voltage.wz, input_cmd_.hight, input_cmd_.status); | ||||||
|  | 
 | ||||||
|  | } | ||||||
|  | 
 | ||||||
|  | } // namespace mdog_simple_controller
 | ||||||
|  | 
 | ||||||
|  | int main(int argc, char **argv) { | ||||||
|  |     rclcpp::init(argc, argv); | ||||||
|  |     rclcpp::spin(std::make_shared<mdog_simple_controller::MdogSimpleController>()); | ||||||
|  |     rclcpp::shutdown(); | ||||||
|  |     return 0; | ||||||
|  | } | ||||||
| @ -12,6 +12,7 @@ find_package(rclcpp_components REQUIRED) | |||||||
| find_package(std_msgs REQUIRED) | find_package(std_msgs REQUIRED) | ||||||
| find_package(serial REQUIRED) | find_package(serial REQUIRED) | ||||||
| find_package(rc_msgs REQUIRED) | find_package(rc_msgs REQUIRED) | ||||||
|  | 
 | ||||||
| # 添加头文件目录 | # 添加头文件目录 | ||||||
| include_directories(include) | include_directories(include) | ||||||
| 
 | 
 | ||||||
|  | |||||||
| @ -8,6 +8,8 @@ | |||||||
| #include <array> | #include <array> | ||||||
| #include "unitree_leg_serial_driver/crc_ccitt.hpp" | #include "unitree_leg_serial_driver/crc_ccitt.hpp" | ||||||
| #include "unitree_leg_serial_driver/gom_protocol.hpp" | #include "unitree_leg_serial_driver/gom_protocol.hpp" | ||||||
|  | #include "rc_msgs/msg/leg_fdb.hpp" | ||||||
|  | #include "rc_msgs/msg/leg_cmd.hpp" | ||||||
| namespace unitree_leg_serial | namespace unitree_leg_serial | ||||||
| { | { | ||||||
| 
 | 
 | ||||||
| @ -19,6 +21,7 @@ public: | |||||||
| 
 | 
 | ||||||
|     void write(const std::array<MotorCmd_t, 12>& cmds); |     void write(const std::array<MotorCmd_t, 12>& cmds); | ||||||
|     std::array<MotorData_t, 12> read(); |     std::array<MotorData_t, 12> read(); | ||||||
|  | 
 | ||||||
| private: | private: | ||||||
|     static constexpr int PORT_NUM = 4; |     static constexpr int PORT_NUM = 4; | ||||||
|     static constexpr int MOTOR_NUM = 3; |     static constexpr int MOTOR_NUM = 3; | ||||||
| @ -30,6 +33,7 @@ private: | |||||||
|     void motor_cmd_reset(int port_idx, int idx); |     void motor_cmd_reset(int port_idx, int idx); | ||||||
|     bool open_serial_port(int port_idx); |     bool open_serial_port(int port_idx); | ||||||
|     void close_serial_port(int port_idx); |     void close_serial_port(int port_idx); | ||||||
|  |     void leg_cmd_callback(const rc_msgs::msg::LegCmd::SharedPtr msg); | ||||||
| 
 | 
 | ||||||
|     int send_count_[PORT_NUM]{}; |     int send_count_[PORT_NUM]{}; | ||||||
|     int recv_count_[PORT_NUM]{}; |     int recv_count_[PORT_NUM]{}; | ||||||
| @ -40,6 +44,8 @@ private: | |||||||
|     rclcpp::TimerBase::SharedPtr timer_; |     rclcpp::TimerBase::SharedPtr timer_; | ||||||
|     std::array<std::thread, PORT_NUM> read_thread_; |     std::array<std::thread, PORT_NUM> read_thread_; | ||||||
|     std::atomic<bool> running_{false}; |     std::atomic<bool> running_{false}; | ||||||
|  |     rclcpp::Publisher<rc_msgs::msg::LegFdb>::SharedPtr leg_fdb_pub_; | ||||||
|  |     rclcpp::Subscription<rc_msgs::msg::LegCmd>::SharedPtr leg_cmd_sub_; | ||||||
| 
 | 
 | ||||||
|     // 状态管理
 |     // 状态管理
 | ||||||
|     enum StatusFlag : int8_t { OFFLINE = 0, ERROR = 1, CONTROLED = 2 }; |     enum StatusFlag : int8_t { OFFLINE = 0, ERROR = 1, CONTROLED = 2 }; | ||||||
|  | |||||||
| @ -1,5 +1,7 @@ | |||||||
| #include "unitree_leg_serial_driver/unitree_leg_serial.hpp" | #include "unitree_leg_serial_driver/unitree_leg_serial.hpp" | ||||||
| #include "rclcpp_components/register_node_macro.hpp" | #include "rclcpp_components/register_node_macro.hpp" | ||||||
|  | #include "rc_msgs/msg/leg_fdb.hpp" | ||||||
|  | #include "rc_msgs/msg/leg_cmd.hpp" | ||||||
| namespace unitree_leg_serial | namespace unitree_leg_serial | ||||||
| { | { | ||||||
| 
 | 
 | ||||||
| @ -11,6 +13,9 @@ UnitreeLegSerial::UnitreeLegSerial(const rclcpp::NodeOptions &options) | |||||||
|     baud_rate_ = {4000000, 4000000, 4000000, 4000000}; |     baud_rate_ = {4000000, 4000000, 4000000, 4000000}; | ||||||
| 
 | 
 | ||||||
|     last_freq_time_ = this->now(); |     last_freq_time_ = this->now(); | ||||||
|  |     leg_fdb_pub_ = this->create_publisher<rc_msgs::msg::LegFdb>("leg_fdb", 10); | ||||||
|  |     leg_cmd_sub_ = this->create_subscription<rc_msgs::msg::LegCmd>( | ||||||
|  |         "leg_cmd", 10, std::bind(&UnitreeLegSerial::leg_cmd_callback, this, std::placeholders::_1)); | ||||||
| 
 | 
 | ||||||
|     for (int p = 0; p < PORT_NUM; ++p) { |     for (int p = 0; p < PORT_NUM; ++p) { | ||||||
|         send_count_[p] = 0; |         send_count_[p] = 0; | ||||||
| @ -85,6 +90,21 @@ std::array<MotorData_t, 12> UnitreeLegSerial::read() | |||||||
|     return result; |     return result; | ||||||
| } | } | ||||||
| 
 | 
 | ||||||
|  | void UnitreeLegSerial::leg_cmd_callback(const rc_msgs::msg::LegCmd::SharedPtr msg) | ||||||
|  | { | ||||||
|  |     std::array<MotorCmd_t, 12> cmds; | ||||||
|  |     for (size_t i = 0; i < 12; ++i) { | ||||||
|  |         cmds[i].T = msg->leg_cmd[i].torque_des; | ||||||
|  |         cmds[i].W = msg->leg_cmd[i].speed_des; | ||||||
|  |         cmds[i].Pos = msg->leg_cmd[i].pos_des; | ||||||
|  |         cmds[i].K_P = msg->leg_cmd[i].kp; | ||||||
|  |         cmds[i].K_W = msg->leg_cmd[i].kd; | ||||||
|  |         cmds[i].id = i % 3; | ||||||
|  |         cmds[i].mode = 1; | ||||||
|  |     } | ||||||
|  |     write(cmds); | ||||||
|  | } | ||||||
|  | 
 | ||||||
| bool UnitreeLegSerial::open_serial_port(int port_idx) | bool UnitreeLegSerial::open_serial_port(int port_idx) | ||||||
| { | { | ||||||
|     try { |     try { | ||||||
| @ -148,6 +168,18 @@ void UnitreeLegSerial::motor_update() | |||||||
|         } |         } | ||||||
|         last_freq_time_ = now; |         last_freq_time_ = now; | ||||||
|     } |     } | ||||||
|  |     rc_msgs::msg::LegFdb msg; | ||||||
|  | 
 | ||||||
|  |     msg.header.stamp = this->now(); | ||||||
|  |     msg.header.frame_id = "leg"; // 可根据实际需要修改
 | ||||||
|  | 
 | ||||||
|  |     auto fbk = read(); | ||||||
|  |     for (size_t i = 0; i < 12; ++i) { | ||||||
|  |         msg.leg_fdb[i].torque = fbk[i].T; | ||||||
|  |         msg.leg_fdb[i].speed = fbk[i].W; | ||||||
|  |         msg.leg_fdb[i].pos = fbk[i].Pos; | ||||||
|  |     } | ||||||
|  |     leg_fdb_pub_->publish(msg); | ||||||
| } | } | ||||||
| 
 | 
 | ||||||
| void UnitreeLegSerial::motor_cmd_reset(int port_idx, int idx) | void UnitreeLegSerial::motor_cmd_reset(int port_idx, int idx) | ||||||
|  | |||||||
| @ -2,6 +2,8 @@ cmake_minimum_required(VERSION 3.8) | |||||||
| project(rc_msgs) | project(rc_msgs) | ||||||
| 
 | 
 | ||||||
| find_package(rosidl_default_generators REQUIRED) | find_package(rosidl_default_generators REQUIRED) | ||||||
|  | find_package(std_msgs REQUIRED) | ||||||
|  | 
 | ||||||
| rosidl_generate_interfaces(${PROJECT_NAME} | rosidl_generate_interfaces(${PROJECT_NAME} | ||||||
|         "msg/DataMCU.msg" |         "msg/DataMCU.msg" | ||||||
|         "msg/DataRef.msg" |         "msg/DataRef.msg" | ||||||
| @ -11,7 +13,9 @@ rosidl_generate_interfaces(${PROJECT_NAME} | |||||||
|         "msg/DataNav.msg" |         "msg/DataNav.msg" | ||||||
|         "msg/GoMotorCmd.msg" |         "msg/GoMotorCmd.msg" | ||||||
|         "msg/GoMotorFdb.msg" |         "msg/GoMotorFdb.msg" | ||||||
|  |         "msg/LegCmd.msg" | ||||||
|  |         "msg/LegFdb.msg" | ||||||
|  |     DEPENDENCIES std_msgs | ||||||
| ) | ) | ||||||
| 
 | 
 | ||||||
| ament_package() | ament_package() | ||||||
| 
 |  | ||||||
|  | |||||||
							
								
								
									
										2
									
								
								src/rc_msgs/msg/LegCmd.msg
									
									
									
									
									
										Normal file
									
								
							
							
						
						
									
										2
									
								
								src/rc_msgs/msg/LegCmd.msg
									
									
									
									
									
										Normal file
									
								
							| @ -0,0 +1,2 @@ | |||||||
|  | std_msgs/Header header | ||||||
|  | GoMotorCmd[12] leg_cmd | ||||||
							
								
								
									
										2
									
								
								src/rc_msgs/msg/LegFdb.msg
									
									
									
									
									
										Normal file
									
								
							
							
						
						
									
										2
									
								
								src/rc_msgs/msg/LegFdb.msg
									
									
									
									
									
										Normal file
									
								
							| @ -0,0 +1,2 @@ | |||||||
|  | std_msgs/Header header | ||||||
|  | GoMotorFdb[12] leg_fdb | ||||||
| @ -5,7 +5,8 @@ | |||||||
|     <description>Describe custom messages</description> |     <description>Describe custom messages</description> | ||||||
|     <maintainer email="1683502971@qq.com">biao</maintainer> |     <maintainer email="1683502971@qq.com">biao</maintainer> | ||||||
|     <license>MIT</license> |     <license>MIT</license> | ||||||
| 
 |     <build_depend>std_msgs</build_depend> | ||||||
|  |     <exec_depend>std_msgs</exec_depend> | ||||||
|     <buildtool_depend>rosidl_default_generators</buildtool_depend> |     <buildtool_depend>rosidl_default_generators</buildtool_depend> | ||||||
|     <exec_depend>rosidl_default_runtime</exec_depend> |     <exec_depend>rosidl_default_runtime</exec_depend> | ||||||
|     <member_of_group>rosidl_interface_packages</member_of_group> |     <member_of_group>rosidl_interface_packages</member_of_group> | ||||||
|  | |||||||
		Loading…
	
		Reference in New Issue
	
	Block a user