houyongdi/dreamDeckAutopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
houyongdi
2026-07-27 13:51:19 +08:00
commit 7bec56ca51
5408 changed files with 1126933 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sensors/bozi_ws/src/CMakeLists.txt
+1
View File
@@ -0,0 +1 @@
/opt/ros/noetic/share/catkin/cmake/toplevel.cmake
sensors/bozi_ws/src/head_servo_controller.zip
BIN
View File
Binary file not shown.
sensors/bozi_ws/src/head_servo_controller/CMakeLists.txt
+39
View File
@@ -0,0 +1,39 @@
cmake_minimum_required(VERSION 3.0.2)
project(head_servo_controller)
find_package(catkin REQUIRED COMPONENTS
autoware_msgs
roscpp
sensor_msgs
serial
message_generation
)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES head_servo_controller
# CATKIN_DEPENDS autoware_msgs roscpp sensor_msgs
# DEPENDS system_lib
)
include_directories(
include
${catkin_INCLUDE_DIRS}
${PROJECT_SOURCE_DIR}/msg
)
add_executable(head_servo_controller_node
src/head_servo_node.cpp
src/head_servo_core.cpp
)
target_link_libraries(head_servo_controller_node
${catkin_LIBRARIES}
)
add_dependencies(head_servo_controller_node
autoware_msgs_generate_messages_cpp
)
sensors/bozi_ws/src/head_servo_controller/include/head_servo_core.h
+98
View File
@@ -0,0 +1,98 @@
#ifndef HEAD_SERVO_CORE_H
#define HEAD_SERVO_CORE_H
#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <thread>
// #include <head_servo_controller/HeadControlCmd.h>
// #include <head_servo_controller/HeadAngleMsg.h>
typedef struct
{
int32_t PU;
} DispModbusData;
class Head_servo
{
private:
ros::NodeHandle nh_;
serial::Serial serial_;
ros::Subscriber control_sub_; // 订阅控制指令
ros::Publisher status_pub_; // 发布状态
// 串口相关配置
std::string head_servo_com_;
std::string __APP_NAME__;
int baudrate_;
// // 串口线程控制
// std::thread serial_thread_;
// bool serial_thread_running = false;
// 控制标志位（对应原STM32代码中的flag）
uint8_t flag_1 = 0; // 查询当前角度
uint8_t flag_2 = 0; // 按速度转到指定角度
uint8_t flag_3 = 0; // 进入巡航模式
uint8_t flag_4 = 0; // 退出巡航模式
uint8_t flag_5 = 0; // 一键回正（90°）
uint8_t flag_6 = 0; // 重启（ROS中可忽略或调用节点重启逻辑）
uint8_t flag_7 = 0; // 设置当前位置为零点
// 目标参数
int16_t target_speed = 0xFFFF; // 0xFFFF表示速度不变
int16_t target_angle = 0xFFFF; // 0xFFFF表示角度不变
// // 通讯数据缓冲区
// uint8_t recvData_Uart3[100];
// uint8_t recvData_Uart2[100];
// // 命令帧定义（根据原代码推测）
// uint8_t ModusEn[8] = {0x01, 0x06, 0x00, 0x00, 0x00, 0x01, 0x48, 0x0A};
// uint8_t param_save[8] = {0x01, 0x06, 0x00, 0x01, 0x00, 0x01, 0xF9, 0xCA};
// uint8_t Motor_speed_target[11] = {0x01, 0x10, 0x00, 0x10, 0x00, 0x01, 0x02, 0x00, 0x00, 0x00, 0x00};
// uint8_t Motor_speed_target_2[11] = {0x01, 0x10, 0x00, 0x10, 0x00, 0x01, 0x02, 0x00, 0x00, 0x00, 0x00};
// uint8_t PosAngle_P135[13] = {0x01, 0x10, 0x00, 0x12, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x87, 0x00, 0x00};
// uint8_t PosAngle_P45[13] = {0x01, 0x10, 0x00, 0x12, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x2D, 0x00, 0x00};
// uint8_t PosAngle_P90[13] = {0x01, 0x10, 0x00, 0x12, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x5A, 0x00, 0x00};
// uint8_t PosAngle_N90[13] = {0x01, 0x10, 0x00, 0x12, 0x00, 0x02, 0x04, 0xFF, 0xFF, 0xFF, 0xA6, 0x00, 0x00};
// ROS话题相关
ros::Subscriber control_cmd_sub_;
ros::Publisher current_angle_pub_;
// CRC校验函数（原代码中使用）
uint16_t usMBCRC16(uint8_t *pucFrame, uint16_t usLen);
// // 串口接收回调函数
// void serialCallback(const ros::TimerEvent& e);
// 控制指令回调函数
// void control_cmd_callback(const head_servo_controller::HeadControlCmd::ConstPtr& msg);
// 核心处理函数（移植原STM32 while(1)中的逻辑）
void process(const ros::TimerEvent &e);
// 控制指令回调
void controlCallback(const std_msgs::String::ConstPtr& msg);
// 辅助功能函数
void position_stop();
void Turn_angle(int16_t angle);
int16_t read_current_angle();
void setCurrentPositionZero();
void fun_response(uint8_t cmd, uint8_t data1, uint8_t data2, uint8_t data3, uint8_t data4);
void calculate_modbus_data(uint16_t modbus_data_11, uint16_t modbus_data_10, DispModbusData *disp_modbus_data);
public:
Head_servo(ros::NodeHandle nh);
~Head_servo();
// 串口初始化
void serial_initial();
// 运行节点
void run();
};
#endif // HEAD_SERVO_CORE_H
sensors/bozi_ws/src/head_servo_controller/launch/head_servo.launch
+13
View File
@@ -0,0 +1,13 @@
<launch>
<arg name="head_servo_com_" default="/dev/ttyACM0"/>
<arg name="baudrate_" default="19200"/>
<arg name="input_ctr_mode_topic_" default="/remote_ctrl"/>
<arg name="input_battery_state_topic_" default="/sensor/battery_state"/>
<node name="head_servo_controller_node" pkg="head_servo_controller" type="head_servo_controller_node" output="screen">
<param name="head_servo_com_" value="$(arg head_servo_com_)" type="string"/>
<param name="baudrate_" value="$(arg baudrate_)" type="int"/>
<param name="input_ctr_mode_topic_" value="$(arg input_ctr_mode_topic_)" type="string"/>
<param name="input_battery_state_topic_" value="$(arg input_battery_state_topic_)" type="string"/>
</node>
</launch>
sensors/bozi_ws/src/head_servo_controller/package.xml
+33
View File
@@ -0,0 +1,33 @@
<?xml version="1.0"?>
<package format="2">
<name>head_servo_controller</name>
<version>0.0.0</version>
<description>The head_servo_controller package</description>
<maintainer email="zhangshu@todo.todo">zhangshu</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>autoware_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>serial</build_depend>
<build_export_depend>autoware_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>serial</build_export_depend>
<exec_depend>autoware_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>serial</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
sensors/bozi_ws/src/head_servo_controller/src/head_mqtt_core.cpp
+149
View File
@@ -0,0 +1,149 @@
#include "head_servo_core.h"
#include <mqtt/async_client.h> // Paho MQTT C++库
#include <thread> // 多线程支持
#include <mutex> // 线程安全锁
// MQTT配置
const std::string MQTT_ADDRESS("tcp://0.0.0.0:1883"); // 本地MQTT服务器地址
const std::string MQTT_CLIENT_ID("HeadServo_MQTT_Server");
const std::string MQTT_TOPIC("head_servo/control"); // 订阅的控制指令主题
// 线程安全锁（保护flag变量）
std::mutex flag_mutex;
// MQTT回调类（处理连接和消息接收）
class MQTTCallback : public virtual mqtt::callback {
private:
Head_servo* head_servo_ptr; // 指向Head_servo实例的指针
public:
MQTTCallback(Head_servo* ptr) : head_servo_ptr(ptr) {}
// 连接丢失回调
void connection_lost(const std::string& cause) override {
ROS_WARN("MQTT连接丢失: %s", cause.c_str());
}
// 消息到达回调（核心：解析MQTT消息并修改flag）
void message_arrived(mqtt::const_message_ptr msg) override {
ROS_INFO("收到MQTT消息: [%s] %s", msg->get_topic().c_str(), msg->to_string().c_str());
// 解析JSON格式的控制指令（示例格式：{"flag_1":1, "flag_2":1, "target_angle":90, ...}）
// 实际应用中可根据需求简化格式（如直接发送flag名称和值）
std::string payload = msg->to_string();
// 线程安全地修改flag（根据消息内容设置对应flag）
std::lock_guard<std::mutex> lock(flag_mutex);
// 示例：解析简单指令（实际需根据通信协议完善）
if (payload.find("query_angle") != std::string::npos) {
head_servo_ptr->flag_1 = 1; // 查询当前角度
} else if (payload.find("set_target") != std::string::npos) {
head_servo_ptr->flag_2 = 1; // 设置目标角度/速度
// 提取目标角度（示例：假设消息中包含"angle:90"）
size_t angle_pos = payload.find("angle:");
if (angle_pos != std::string::npos) {
head_servo_ptr->target_angle = std::stoi(payload.substr(angle_pos + 6));
}
} else if (payload.find("enter_cruise") != std::string::npos) {
head_servo_ptr->flag_3 = 1; // 进入巡航模式
} else if (payload.find("exit_cruise") != std::string::npos) {
head_servo_ptr->flag_4 = 1; // 退出巡航模式
} else if (payload.find("home_position") != std::string::npos) {
head_servo_ptr->flag_5 = 1; // 返回90°
} else if (payload.find("set_zero") != std::string::npos) {
head_servo_ptr->flag_7 = 1; // 设置当前位置为零点
}
}
// 消息发送完成回调
void delivery_complete(mqtt::delivery_token_ptr token) override {
ROS_DEBUG("MQTT消息发送完成");
}
};
// MQTT连接监听器
class ActionListener : public virtual mqtt::iaction_listener {
private:
void on_failure(const mqtt::token& tok) override {
ROS_WARN("MQTT操作失败");
}
void on_success(const mqtt::token& tok) override {
ROS_INFO("MQTT操作成功");
}
};
// 扩展Head_servo类，添加MQTT相关成员
class HeadServoWithMQTT : public Head_servo {
public:
mqtt::async_client mqtt_client; // MQTT客户端
MQTTCallback mqtt_callback; // MQTT回调实例
ActionListener mqtt_listener; // MQTT连接监听器
// 构造函数
HeadServoWithMQTT(ros::NodeHandle nh)
: Head_servo(nh),
mqtt_client(MQTT_ADDRESS, MQTT_CLIENT_ID),
mqtt_callback(this) {}
// 初始化MQTT服务器
void mqtt_init() {
// 设置MQTT回调
mqtt_client.set_callback(mqtt_callback);
// 配置连接选项
mqtt::connect_options conn_opts;
conn_opts.set_keep_alive_interval(20); // 心跳间隔20秒
conn_opts.set_clean_session(true); // 清理会话
// 连接MQTT服务器
try {
ROS_INFO("连接MQTT服务器: %s", MQTT_ADDRESS.c_str());
mqtt::token_ptr conntok = mqtt_client.connect(conn_opts);
conntok->wait(); // 等待连接完成
ROS_INFO("MQTT服务器连接成功");
// 订阅控制指令主题
mqtt_client.subscribe(MQTT_TOPIC, 1, nullptr, mqtt_listener);
ROS_INFO("已订阅MQTT主题: %s", MQTT_TOPIC.c_str());
} catch (const mqtt::exception& e) {
ROS_ERROR("MQTT初始化失败: %s", e.what());
exit(1);
}
}
// 重写run()函数，同时启动ROS和MQTT
void run() override {
serial_initial(); // 初始化串口
mqtt_init(); // 初始化MQTT
// 创建线程运行MQTT循环（非阻塞）
std::thread mqtt_thread([this]() {
while (ros::ok()) {
// 处理MQTT消息（非阻塞模式）
mqtt_client.loop(100); // 超时100ms，避免阻塞
ros::Duration(0.01).sleep(); // 短暂休眠
}
});
// 启动ROS定时器和主循环
ros::Timer timer = nh_.createTimer(ros::Duration(0.01), &Head_servo::process, this);
ros::spin();
// 退出时清理
mqtt_client.disconnect()->wait(); // 断开MQTT连接
mqtt_thread.join(); // 等待MQTT线程结束
}
};
// 主函数
int main(int argc, char**argv) {
ros::init(argc, argv, "head_servo_with_mqtt");
ros::NodeHandle nh("~");
// 创建带MQTT功能的节点实例并运行
HeadServoWithMQTT node(nh);
node.run();
return 0;
}
sensors/bozi_ws/src/head_servo_controller/src/head_servo_core.cpp
+733
View File
@@ -0,0 +1,733 @@
#include "head_servo_core.h"
#include <jsoncpp/json/json.h>
#include <nlohmann/json.hpp>
using json = nlohmann::json;
/* 以下逆时针需要用到32768的负数那种，可以通过covertPU这个函数计算得到 */
/* 位置模式： 转动到指定位置模板 */
static uint8_t Angle_Target[] = {0x01, 0x10, 0x00, 0x16, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
/* 位置模式： 顺时针到180° */
static uint8_t PosAngle_P180[] = {0x01, 0x10, 0x00, 0x16, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x02, 0xF3, 0x48};
/* 位置模式：45° */
static uint8_t PosAngle_P45[] = {0X01, 0X10, 0X00, 0X16, 0X00, 0X02, 0X04, 0X80, 0X00, 0X00, 0X00, 0X5B, 0X49};
/* 位置模式：135° */
static uint8_t PosAngle_P135[] = {0x01, 0x10, 0x00, 0x16, 0x00, 0x02, 0x04, 0x80, 0x00, 0x00, 0x01, 0x9A, 0x89};
/* 位置模式： 1° */
static uint8_t PosAngle_N1[] = {0x01, 0x10, 0x00, 0x16, 0x00, 0x02, 0x04, 0x00, 0x01, 0x00, 0x00, 0x23, 0x49};
/* 位置模式：顺时针转到90° */
static uint8_t PosAngle_P90[] = {0x01, 0x10, 0x00, 0x16, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x01, 0xB3, 0x49};
/* modbus使能 */
static uint8_t ModusEn[] = {0x01, 0x06, 0x00, 0x00, 0x00, 0x01, 0x48, 0x0A};
/* 发送目标速度 */
static uint8_t Motor_speed_target[] = {0x01, 0x06, 0x00, 0x02, 0x00, 0xC8, 0x29, 0x9C};
/* 发送目标速度 */
static uint8_t Motor_speed_target_2[] = {0x01, 0x06, 0x00, 0x02, 0x00, 0x28, 0x28, 0x14};
/* 发送参数保存标志 */
static uint8_t param_save[] = {0x01, 0x06, 0x00, 0x14, 0x00, 0x01, 0x08, 0x0E};
/* 电子齿轮发0 */
static uint8_t Motor_mole_zero[] = {0x01, 0x06, 0x00, 0x0A, 0x00, 0x00, 0xA9, 0xC8};
/* 电子齿轮发 60006 */
static uint8_t Motor_mole_set_zero_1[] = {0x01, 0x06, 0x00, 0x0A, 0xEA, 0x66, 0x66, 0x82};
/* 电子齿轮发 60016 */
static uint8_t Motor_mole_set_zero_2[] = {0x01, 0x06, 0x00, 0x0A, 0xEA, 0x70, 0xE7, 0x4C};
/* 增量位置发0 */
static uint8_t Motor_incre_zero[] = {0x01, 0x10, 0x00, 0x0C, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x00, 0xF3, 0xFA};
/* CRC16 数组 */
static const uint8_t aucCRCHi[] = {0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00,
0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80,
0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80,
0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00,
0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40};
static const uint8_t aucCRCLo[] = {0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D,
0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB,
0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2,
0x12, 0x13, 0xD3, 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37,
0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8,
0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24,
0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63,
0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE,
0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F,
0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71,
0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C,
0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89,
0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46,
0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40};
Head_servo::Head_servo(ros::NodeHandle nh) : nh_(nh)
{
// Get ROS params
nh_.param<std::string>("head_servo_com", head_servo_com_, "/dev/ttyS0");
nh_.param<std::string>("app_name", __APP_NAME__, "head_servo_controller");
nh_.param<int>("baudrate_", baudrate_, 19200);
// 初始化控制标志
flag_1 = 0;
flag_2 = 0;
flag_3 = 0;
flag_4 = 0;
flag_5 = 0;
flag_7 = 0;
target_speed = 0xFFFF;
target_angle = 0xFFFF;
// 初始化订阅者和发布者
control_sub_ = nh_.subscribe("/head_servo/control_cmd", 10, &Head_servo::controlCallback, this);
status_pub_ = nh_.advertise<std_msgs::String>("/head_servo/status", 10);
}
// // 控制指令解析回调
// void Head_servo::controlCallback(const std_msgs::String::ConstPtr& msg)
// {
// std::string cmd = msg->data;
// ROS_INFO("[%s] 收到控制指令: %s", __APP_NAME__.c_str(), cmd.c_str());
// // 解析命令并设置对应标志
// if (cmd == "query_angle") {
// flag_1 = 1; // 查询当前角度
// } else if (cmd == "enter_cruise") {
// flag_3 = 1; // 进入巡航模式
// } else if (cmd == "exit_cruise") {
// flag_4 = 1; // 退出巡航模式
// } else if (cmd == "home_position") {
// flag_5 = 1; // 返回90°
// } else if (cmd == "set_zero") {
// flag_7 = 1; // 设置零点
// // 解析带参数的命令
// } else if (cmd.find("set_target") != std::string::npos) {
// flag_2 = 1; // 设置目标角度/速度
// // 提取目标角度和速度（格式示例："set_target,angle:90,speed:100"）
// size_t angle_pos = cmd.find("angle:");
// size_t speed_pos = cmd.find("speed:");
// if (angle_pos != std::string::npos) {
// std::string angle_str = cmd.substr(angle_pos + 6);
// angle_str = angle_str.substr(0, angle_str.find(','));
// target_angle = std::stoi(angle_str);
// }
// if (speed_pos != std::string::npos) {
// std::string speed_str = cmd.substr(speed_pos + 6);
// target_speed = std::stoi(speed_str);
// }
// }
// // 发布状态消息
// std_msgs::String status_msg;
// status_msg.data = "Command received: " + cmd;
// status_pub_.publish(status_msg);
// }
// mosquitto_pub -t "head_servo/control" -m '{"command":"set_target","angle":0,"speed":40}'
// mosquitto_pub -t "head_servo/control" -m '{"command":"query_angle"}'
// mosquitto_pub -t "head_servo/control" -m '{"command":"reset_zero"}'
// mosquitto_pub -t "head_servo/control" -m '{"command":"enter_cruise"}'
// mosquitto_pub -t "head_servo/control" -m '{"command":"stop"}'
/*
主题： ‘robot/yuntai_control/{device_id}’
Qos: 2
消息内容：
绝对位置角度控制：
{
"uuid": "f47ac10b-58cc-4372-a567-0e02b2c3d465",
"task_type:": "yuntai_control",
"command": "angle",
"data":{
"angle": 15, # 0-180度。0对应最左边，180对应最右边
"speed": 30, # 060度。最⼤移动速度。不传值则为默认30度。
}
}
相对位置角度控制：
{
"uuid": "f47ac10b-58cc-4372-a567-0e02b2c3d465",
"task_type:": "yuntai_control",
"command": "direction",
"data":{
"direction": "left", # "left"/"right", 向左/向右移动指定⻆度
"angle": 15, # 5-30度。移动的度数。不传值则为默认15度
}
}
循环旋转控制：
{
"uuid": "f47ac10b-58cc-4372-a567-0e02b2c3d465",
"task_type:": "yuntai_control",
"command": "loop",
"data": {
"speed": 30, # 0-60度。最⼤移动速度。不传值则为默认30度。
}
}
回中控制：
{
"uuid": "f47ac10b58cc4372a5670e02b2c3d465",
"tasktype:": "yuntaicontrol",
"command": "center",
"data": {
"speed": 30, # 060度。最⼤移动速度。不传值则为默认30度。
}
}
*/
// 修改 C++ 代码中的控制回调函数
void Head_servo::controlCallback(const std_msgs::String::ConstPtr& msg)
{
std::string cmd = msg->data;
ROS_INFO("[%s] 收到控制指令: %s", __APP_NAME__.c_str(), cmd.c_str());
try {
// 尝试解析 JSON 命令
json root = json::parse(cmd);
// 处理 JSON 格式命令
if (root.contains("command")) {
std::string command = root["command"].get<std::string>();
if (command == "query_angle") {
flag_1 = 1;
ROS_INFO("1");
} else if (command == "enter_cruise") {
flag_3 = 1;
ROS_INFO("3");
} else if (command == "set_target") {
flag_2 = 1;
ROS_INFO("2");
if (root.contains("angle"))
target_angle = root["angle"].get<int>();
if (root.contains("speed"))
target_speed = root["speed"].get<int>();
} else if (command == "reset_zero") {
flag_7 = 1;
ROS_INFO("7");
} else if (command == "stop") {
flag_4 = 1;
ROS_INFO("4");
}
// 其他命令...
}
} catch (const json::parse_error& e) {
// 处理JSON解析错误
ROS_WARN("[%s] 解析JSON命令时出错: %s", __APP_NAME__.c_str(), e.what());
} catch (const json::type_error& e) {
// 处理类型错误
ROS_WARN("[%s] JSON类型不匹配: %s", __APP_NAME__.c_str(), e.what());
} catch (const std::exception& e) {
// 处理其他异常
ROS_WARN("[%s] 处理命令时出错: %s", __APP_NAME__.c_str(), e.what());
}
// try {
// // 尝试解析 JSON 命令
// Json::Value root;
// Json::Reader reader;
// if (reader.parse(cmd, root)) {
// // 处理 JSON 格式命令
// if (root.isMember("command")) {
// std::string command = root["command"].asString();
// if (command == "query_angle") {
// flag_1 = 1;
// } else if (command == "enter_cruise") {
// flag_3 = 1;
// } else if (command == "set_target") {
// flag_2 = 1;
// if (root.isMember("angle"))
// target_angle = root["angle"].asInt();
// if (root.isMember("speed"))
// target_speed = root["speed"].asInt();
// }
// // 其他命令...
// }
// } else {
// // 处理普通字符串命令（保持原有逻辑）
// // ...
// }
// } catch (const std::exception& e) {
// ROS_WARN("[%s] 解析命令时出错: %s", __APP_NAME__.c_str(), e.what());
// }
// 发布状态消息
std_msgs::String status_msg;
status_msg.data = "Command received: " + cmd;
status_pub_.publish(status_msg);
}
Head_servo::~Head_servo()
{
if (serial_.isOpen())
{
serial_.close();
}
}
// CRC16 check implementation
uint16_t Head_servo::usMBCRC16(uint8_t *pucFrame, uint16_t usLen)
{
uint8_t ucCRCHi = 0xFF;
uint8_t ucCRCLo = 0xFF;
int iIndex;
while (usLen--)
{
iIndex = ucCRCLo ^ *(pucFrame++);
ucCRCLo = (uint8_t)(ucCRCHi ^ aucCRCHi[iIndex]);
ucCRCHi = aucCRCLo[iIndex];
}
return (uint16_t)(ucCRCLo << 8 | ucCRCHi);
}
// Serial init
void Head_servo::serial_initial()
{
serial::Timeout timeOut = serial::Timeout::simpleTimeout(1000);
try
{
serial_.setPort(head_servo_com_);
serial_.setBaudrate(baudrate_);
serial_.setTimeout(timeOut);
serial_.open();
}
catch (serial::IOException &e)
{
ROS_ERROR("[%s] Serial open failed: %s", __APP_NAME__.c_str(), e.what());
return;
}
if (serial_.isOpen())
{
ROS_INFO("[%s] Serial init success, starting init sequence.", __APP_NAME__.c_str());
// Send 485 enable
serial_.write(ModusEn, sizeof(ModusEn));
ros::Duration(0.01).sleep();
serial_.write(param_save, sizeof(param_save));
ros::Duration(0.01).sleep();
// Init speed
serial_.write(Motor_speed_target_2, sizeof(Motor_speed_target_2));
ros::Duration(0.005).sleep();
serial_.write(param_save, sizeof(param_save));
ros::Duration(0.005).sleep();
// Stop multiple times
for (uint8_t i = 0; i < 100; i++)
{
position_stop();
ros::Duration(0.02).sleep();
}
// // Return to 0
Turn_angle(0);
ros::Duration(1.0).sleep();
ROS_INFO("[%s] Init done, waiting for commands", __APP_NAME__.c_str());
}
}
// // Control cmd callback
// void Head_servo::control_cmd_callback(const head_servo_controller::HeadControlCmd::ConstPtr& msg)
// {
// // Set flags per msg fields
// if (msg->query_angle) flag_1 = 1;
// if (msg->set_target) { flag_2 = 1; target_speed = msg->target_speed; target_angle = msg->target_angle; }
// if (msg->enter_cruise) flag_3 = 1;
// if (msg->exit_cruise) flag_4 = 1;
// if (msg->home_position) flag_5 = 1;
// if (msg->set_zero) flag_7 = 1;
// ROS_INFO("[%s] Recv cmd: query=%d, set=%d, enter_cruise=%d, exit_cruise=%d, home=%d, set_zero=%d",
// __APP_NAME__.c_str(), msg->query_angle, msg->set_target, msg->enter_cruise,
// msg->exit_cruise, msg->home_position, msg->set_zero);
// }
// Position stop func
void Head_servo::position_stop()
{
serial_.write(Motor_mole_zero, sizeof(Motor_mole_zero));
ros::Duration(0.05).sleep();
serial_.write(Motor_incre_zero, sizeof(Motor_incre_zero));
ros::Duration(0.05).sleep();
}
// Turn to specified angle
void Head_servo::Turn_angle(int16_t target_angle)
{
ROS_INFO("[0", __APP_NAME__.c_str());
// target_angle = target_angle > 135 ? 135 : target_angle;
// target_angle = target_angle < 45 ? 45 : target_angle;
// 目标角度不能是0度，最多到1度
// (对于脖子来说0度和1度无所谓，保证正确，0度就变成1度，，，其他而言，零度发1个脉冲，而不是1度)
target_angle = target_angle == 0 ? 1 : target_angle;
// 读取当前角度
// int16_t current_angle = read_current_angle();
// while (current_angle == 0x7FFF)
// {
// current_angle = read_current_angle();
// }
// printf("current angle is %d\r\n", current_angle);
// 转换成要发送给电机的数据
int32_t po_angle = 0xFFFF;
if (target_angle<0)
{
po_angle = -(-target_angle * (32768 * 8) / 360);
}
else if(target_angle>0){
po_angle = target_angle * (32768 * 8) / 360;
}
// 构件要发送的帧数据
Angle_Target[8] = po_angle & 0xFF;
Angle_Target[7] = (po_angle >> 8) & 0xFF;
Angle_Target[10] = (po_angle >> 16) & 0xFF;
Angle_Target[9] = (po_angle >> 24) & 0xFF;
uint16_t crc_temp = usMBCRC16(Angle_Target, 11);
Angle_Target[11] = (crc_temp >> 8) & 0xFF;
Angle_Target[12] = crc_temp & 0xFF;
/* 转到指定角度 */
serial_.write(Angle_Target, sizeof(Angle_Target));
ros::Duration(0.001).sleep();
serial_.write(Angle_Target, sizeof(Angle_Target));
ROS_INFO("[1", __APP_NAME__.c_str());
ros::Duration(7).sleep();
/* 每隔1ms查询一次当前位置，到达目标位置为止 */
/* 发送命令，读取当前位置以作验证 */
// int16_t angle = 0x7FFF;
// int16_t de_angle = 0x7FFF;
// // flag_count_1s = 0;
// while (1)
// {
// // if (flag_count_1s >= 20)
// // {
// // flag_count_1s = 0;
// // break;
// // }
// angle = read_current_angle();
// de_angle = (target_angle - angle);
// de_angle = de_angle > 0 ? de_angle : -de_angle;
// if (de_angle >= 0 && de_angle <= 3)
// {
// break;
// }
// ros::Duration(0.001).sleep();
// }
}
/*
将两个16位数据转换成一个32位数据
11是高16位，10是低16位
左移优先级很低，使用时记得加括号
*/
void Head_servo::calculate_modbus_data(uint16_t modbus_data_11, uint16_t modbus_data_10, DispModbusData *disp_modbus_data)
{
// printf("%d\t\t%d\r\n", modbus_data_11, modbus_data_10);
if (modbus_data_11 > 32767)
{ // If high byte exceeds 32767, interpret as negative value
// Calculate PU for negative value
disp_modbus_data->PU = ((modbus_data_11)-32768) * 65536 + modbus_data_10;
// Convert to negative value (two's complement)
disp_modbus_data->PU = -((0x7FFFFFFF - disp_modbus_data->PU) + 1);
}
else
{
// Calculate PU for positive value
disp_modbus_data->PU = modbus_data_11 * 65536 + modbus_data_10;
}
}
// Read current angle
int16_t Head_servo::read_current_angle()
{
// Send read angle cmd
uint8_t read_cmd[] = {0x01, 0x03, 0x00, 0x16, 0x00, 0x02, 0x25, 0xCF};
DispModbusData recvData = {0};
serial_.write(read_cmd, sizeof(read_cmd));
// Wait for response
// ros::Time start_time = ros::Time::now();
// while ((ros::Time::now() - start_time).toSec() < 0.1) // 100ms timeout
// {
// ros::Duration(0.003).sleep();
if (serial_.available() >= 7) // Min 7 bytes response
{
uint8_t buffer[100];
// size_t bytes_read = serial_.read(buffer, serial_.available());
size_t bytes_read = serial_.read(buffer, std::min(serial_.available(), (size_t)100));
// 打印原始数据（以十六进制格式）
ROS_INFO("[%s] 收到串口数据 (%zu bytes):", __APP_NAME__.c_str(), bytes_read);
for (size_t i = 0; i < bytes_read; i++) {
ROS_INFO(" [%02zu]: 0x%02X", i, buffer[i]);
}
// Simple parse (assumed correct format)
if (bytes_read >= 7 && buffer[0] == 0x01 && buffer[1] == 0x03)
{
// Extract angle (high byte first)
int16_t angle_1 = (buffer[3] << 8) | buffer[4];
int16_t angle_2 = (buffer[5] << 8) | buffer[6];
calculate_modbus_data(angle_2, angle_1, &recvData);
return (recvData.PU * 360 / (32768 * 8));
}
}
return 0x7FFF;
ros::Duration(0.001).sleep();
// }
// return 0x7FFF; // Invalid on timeout
}
// Set current pos as zero
void Head_servo::setCurrentPositionZero()
{
serial_.write(Motor_mole_set_zero_1, sizeof(Motor_mole_set_zero_1));
ros::Duration(0.050).sleep();
serial_.write(Motor_mole_set_zero_2, sizeof(Motor_mole_set_zero_2));
ros::Duration(0.050).sleep();
serial_.write(param_save, sizeof(param_save));
ros::Duration(0.050).sleep();
}
// Response func (replace original fun_response)
void Head_servo::fun_response(uint8_t cmd, uint8_t data1, uint8_t data2, uint8_t data3, uint8_t data4)
{
// Build response frame
uint8_t response[] = {0x01, cmd, 0x04, data1, data2, data3, data4, 0x00, 0x00};
// Calc CRC
uint16_t crc = usMBCRC16(response, 7);
response[7] = (crc >> 8) & 0xFF;
response[8] = crc & 0xFF;
// Send response
serial_.write(response, 9);
}
// Core process func
void Head_servo::process(const ros::TimerEvent &e)
{
if (!serial_.isOpen()) return;
// Handle flag_6 (restart, log only)
// 这里不是单片机，不需要重启
// if (flag_6 == 1)
// {
// ROS_WARN("[%s] Recv restart cmd", __APP_NAME__.c_str());
// flag_6 = 0;
// return;
// }
// Handle flag_1: query angle
// ROS_INFO("process");
if (flag_1 == 1)
{
ROS_INFO("1");
int16_t angle = read_current_angle();
while (angle == 0x7FFF) // Wait for valid
{
angle = read_current_angle();
ROS_INFO("2");
ros::Duration(0.01).sleep();
}
ROS_INFO("[%s] Current angle: %d°", __APP_NAME__.c_str(), angle);
flag_1 = 0;
}
// Handle flag_2: turn to angle at speed
if (flag_2 == 1)
{
ROS_INFO("[%s] Recv target: speed=%d, angle=%d", __APP_NAME__.c_str(), target_speed, target_angle);
// target_angle 是从外部获取得到
if ((target_angle <-45 || target_angle > 45) && target_angle != 0xFFFF)
{
ROS_WARN("[%s] Angle out of range (-45 to 45)", __APP_NAME__.c_str());
flag_2 = 0;
return;
}
// Handle speed setting
if (target_speed != 0xFFFF)
{
uint8_t Motor_speed_target_copy[8];
memcpy(Motor_speed_target_copy, Motor_speed_target, sizeof(Motor_speed_target));
// Update speed
Motor_speed_target_copy[4] = (target_speed >> 8) & 0xFF;
Motor_speed_target_copy[5] = target_speed & 0xFF;
// Recalc CRC
uint16_t crc = usMBCRC16(Motor_speed_target_copy, 6);
Motor_speed_target_copy[6] = (crc >> 8) & 0xFF;
Motor_speed_target_copy[7] = crc & 0xFF;
// Send cmd
serial_.write(Motor_speed_target_copy, sizeof(Motor_speed_target_copy));
ros::Duration(0.005).sleep();
serial_.write(param_save, sizeof(param_save));
ros::Duration(0.005).sleep();
ROS_INFO("[%s] Speed set: %d", __APP_NAME__.c_str(), target_speed);
}
// Handle angle setting
if (target_angle != 0xFFFF)
{
position_stop(); // Stop first
ros::Duration(0.01).sleep();
Turn_angle(target_angle); // Turn to target
position_stop(); // Stop after arrival
ROS_INFO("[%s] Turned to: %d°", __APP_NAME__.c_str(), target_angle);
}
flag_2 = 0;
}
// Handle flag_3: enter cruise (45°↔135°)
if (flag_3 == 1)
{
ROS_INFO("[%s] Enter cruise mode", __APP_NAME__.c_str());
if(flag_4==1){
flag_3 = 0;
return ;
}
Turn_angle(30);
if(flag_4==1){
flag_3 = 0;
return ;
}
Turn_angle(-30);
if(flag_4==1) {
flag_3 = 0;
return ;
}
// // Turn to 135°
// position_stop();
// ros::Duration(0.001).sleep();
// serial_.write(PosAngle_P135, sizeof(PosAngle_P135));
// ros::Duration(0.001).sleep();
// serial_.write(PosAngle_P135, sizeof(PosAngle_P135));
// // Wait to reach 135° (≤3° error)
// /* 每隔1ms查询一次当前位置（读取当前位置中有个20ms延时），到达目标位置为止 */
// /* 发送命令，读取当前位置以作验证 */
// int16_t angle = 0x7FFF;
// int16_t de_angle = 0x7FFF; // 当前位置与目标位置的偏差
// int16_t last_angle = 0x7FFF;
// while (1)
// {
// // 终端中收到了停止指令，进入下一轮主循环
// if (flag_4 == 1)
// {
// flag_3 = 0;
// return;
// }
// angle = read_current_angle();
// de_angle = (135 - angle);
// de_angle = de_angle > 0 ? de_angle : -de_angle;
// if (de_angle >= 0 && de_angle <= 3)
// {
// break;
// }
// ros::Duration(0.001).sleep();
// }
// /* 逆时针转到45° */
// ros::Duration(0.001).sleep();
// serial_.write(PosAngle_P45, sizeof(PosAngle_P135));
// ros::Duration(0.001).sleep();
// serial_.write(PosAngle_P45, sizeof(PosAngle_P135));
// /* 每隔1ms查询一次当前位置，到达目标位置为止 */
// /* 发送命令，读取当前位置以作验证 */
// angle = 0x7FFF;
// de_angle = 0x7FFF;
// last_angle = 0x7FFF;
// while (1)
// {
// if (flag_4 == 1)
// {
// flag_3 = 0;
// return;
// }
// angle = read_current_angle();
// de_angle = (45 - angle);
// de_angle = de_angle > 0 ? de_angle : -de_angle;
// if (de_angle >= 0 && de_angle <= 3)
// {
// break;
// }
// ros::Duration(0.001).sleep();
// }
}
// Handle flag_4: exit cruise
if (flag_4 == 1)
{
position_stop();
ROS_INFO("[%s] Exited cruise mode", __APP_NAME__.c_str());
flag_4 = 0;
}
// Handle flag_5: return to 90°
if (flag_5 == 1)
{
ROS_INFO("[%s] Return to 90°", __APP_NAME__.c_str());
Turn_angle(0);
flag_5 = 0;
}
// Handle flag_7: set current as zero
if (flag_7 == 1)
{
ROS_INFO("[%s] Set current pos as zero", __APP_NAME__.c_str());
setCurrentPositionZero();
flag_7 = 0;
}
}
// Run node
void Head_servo::run()
{
serial_initial(); // Init serial and hardware
// Init topics
// control_cmd_sub_ = nh_.subscribe("head_control_cmd", 1, &Head_servo::control_cmd_callback, this);
// current_angle_pub_ = nh_.advertise<head_servo_controller::HeadAngleMsg>("current_angle", 1);
// Start timer (run process every 10ms)
ros::Timer timer = nh_.createTimer(ros::Duration(0.01), &Head_servo::process, this);
// ROS spin
ros::spin();
}
// Main
int main(int argc, char **argv)
{
ros::init(argc, argv, "head_servo_controller");
ros::NodeHandle nh("~");
Head_servo head_servo(nh);
head_servo.run();
return 0;
}
sensors/bozi_ws/src/head_servo_controller/src/head_servo_node.cpp
+10
View File
@@ -0,0 +1,10 @@
// #include "head_servo_core.h"
// int main(int argc, char **argv){
// ros::init(argc, argv, "head_servo_node");
// ros::NodeHandle nh;
// Head_servo head_servo_contrl(nh);
// ros::spin();
// // head_servo_contrl.run();
// return 0;
// }