cxh/sweeper_e800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Auto commit at 2025-06-11 16:29:14

Browse Source
This commit is contained in:
cxh 2025-06-11 16:29:14 +08:00
parent 9d3daf70fe
commit 80201e18a9
22 changed files with 277 additions and 574 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/ctrl_arbiter/CMakeLists.txt
View File

@ -8,7 +8,7 @@ endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(mc REQUIRED)
find_package(sweeper_interfaces REQUIRED)
find_package(std_msgs REQUIRED)
file(GLOB SRC_FILES "src/*.cpp")
@ -17,7 +17,7 @@ add_executable(ctrl_arbiter_node ${SRC_FILES})
ament_target_dependencies(ctrl_arbiter_node
rclcpp
mc
sweeper_interfaces
std_msgs
)

3
src/ctrl_arbiter/package.xml
View File

@ -9,9 +9,8 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>sweeper_interfaces</depend>
<depend>rclcpp</depend>
<depend>mc</depend>
<depend>std_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

32
src/ctrl_arbiter/src/ctrl_arbiter.cpp
View File

@ -1,9 +1,10 @@
#include "rclcpp/rclcpp.hpp"
#include "mc/msg/mc_ctrl.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
#include <chrono>
#include <mutex>
using namespace std::chrono_literals;
namespace sweeperMsg = sweeper_interfaces::msg;
class ArbitrationNode : public rclcpp::Node
{
@ -13,11 +14,11 @@ public:
{
timeout_ms_ = 200; // 200毫秒超时阈值，可调整
publisher_ = this->create_publisher<mc::msg::McCtrl>("/mc_ctrl", 10);
publisher_ = this->create_publisher<sweeperMsg::McCtrl>("/mc_ctrl", 10);
sub_radio_ = this->create_subscription<mc::msg::McCtrl>(
sub_radio_ = this->create_subscription<sweeperMsg::McCtrl>(
"/radio_mc_ctrl", 10,
[this](const mc::msg::McCtrl::SharedPtr msg)
[this](const sweeperMsg::McCtrl::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);
radio_msg_ = *msg;
@ -25,9 +26,9 @@ public:
radio_valid_ = true;
});
sub_remote_ = this->create_subscription<mc::msg::McCtrl>(
sub_remote_ = this->create_subscription<sweeperMsg::McCtrl>(
"/remote_mc_ctrl", 10,
[this](const mc::msg::McCtrl::SharedPtr msg)
[this](const sweeperMsg::McCtrl::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);
remote_msg_ = *msg;
@ -35,9 +36,9 @@ public:
remote_valid_ = true;
});
sub_auto_ = this->create_subscription<mc::msg::McCtrl>(
sub_auto_ = this->create_subscription<sweeperMsg::McCtrl>(
"/auto_mc_ctrl", 10,
[this](const mc::msg::McCtrl::SharedPtr msg)
[this](const sweeperMsg::McCtrl::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);
auto_msg_ = *msg;
@ -73,32 +74,27 @@ private:
}
// 所有控制源均失联，发布安全默认指令
mc::msg::McCtrl safe_msg;
safe_msg.mcu_enabled = true;
sweeperMsg::McCtrl safe_msg;
safe_msg.brake = 1;
safe_msg.gear = 0;
safe_msg.rpm = 0;
safe_msg.brake_time_ms = 500;
safe_msg.angle = 0;
safe_msg.angle_speed = 120;
safe_msg.main_brush_lift = false;
safe_msg.edge_brush_lift = false;
safe_msg.vacuum = false;
safe_msg.sweep_ctrl = false;
safe_msg.spray = false;
safe_msg.mud_flap = false;
safe_msg.dust_shake = false;
safe_msg.main_brush_spin = false;
safe_msg.edge_brush_spin = false;
publisher_->publish(safe_msg);
}
rclcpp::Publisher<mc::msg::McCtrl>::SharedPtr publisher_;
rclcpp::Publisher<sweeperMsg::McCtrl>::SharedPtr publisher_;
rclcpp::Subscription<mc::msg::McCtrl>::SharedPtr sub_radio_, sub_remote_, sub_auto_;
rclcpp::Subscription<sweeperMsg::McCtrl>::SharedPtr sub_radio_, sub_remote_, sub_auto_;
rclcpp::TimerBase::SharedPtr timer_;
mc::msg::McCtrl radio_msg_, remote_msg_, auto_msg_;
sweeperMsg::McCtrl radio_msg_, remote_msg_, auto_msg_;
rclcpp::Time radio_last_time_, remote_last_time_, auto_last_time_;
bool radio_valid_ = false;
bool remote_valid_ = false;

22
src/mc/CMakeLists.txt
View File

@ -6,16 +6,10 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
endif()
# Find dependencies
find_package(sweeper_interfaces REQUIRED)
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)
# Generate message
rosidl_generate_interfaces(${PROJECT_NAME} "msg/CanFrame.msg" "msg/McCtrl.msg"
DEPENDENCIES std_msgs)
ament_export_dependencies(rosidl_default_runtime)
# 搜索 src 目录下所有 .cpp 文件
file(GLOB SRC_FILES src/*.cpp)
@ -23,33 +17,23 @@ file(GLOB SRC_FILES src/*.cpp)
# 创建可执行文件
add_executable(mc_node ${SRC_FILES})
ament_target_dependencies(mc_node rclcpp std_msgs)
rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME}
"rosidl_typesupport_cpp")
target_link_libraries(mc_node ${cpp_typesupport_target})
ament_target_dependencies(mc_node rclcpp std_msgs sweeper_interfaces)
# Set include directories for the target
target_include_directories(
mc_node
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_cpp>
$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/rosidl_typesupport_cpp>
$<INSTALL_INTERFACE:include>)
target_compile_features(mc_node PUBLIC c_std_99 cxx_std_17) # Require C99 and
# C++17
target_compile_features(mc_node PUBLIC c_std_99 cxx_std_17)
# Install target
install(TARGETS mc_node DESTINATION lib/${PROJECT_NAME})
# 安装配置目录
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
# Handle testing if needed
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# Skip linter checking for copyrights if not relevant
set(ament_cmake_copyright_FOUND TRUE)
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()

13
src/mc/include/mc/can_driver.h
View File

@ -6,18 +6,12 @@
#include <vector>
#include <thread>
#include <atomic>
#ifdef _WIN32
#include <windows.h>
// 这里需要包含Windows平台特定的CAN头文件，例如PEAKCAN等
#else
#include <linux/can.h>
#include <linux/can/raw.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <unistd.h>
#endif
struct CANFrame
{
@ -60,13 +54,8 @@ public:
private:
void receiveThreadFunc();
bool applyFilters(); // 应用当前filters_
#ifdef _WIN32
// Windows平台相关成员
HANDLE hCAN = INVALID_HANDLE_VALUE;
#else
int sockfd = -1;
#endif
int sockfd = -1;
std::atomic<bool> running{false};
std::thread receiveThread;
ReceiveCallback callback;

283
src/mc/include/mc/can_struct.h
View File

@ -12,27 +12,27 @@ union can_MCU_cmd_union
{
struct
{
// Byte 0
uint8_t brake : 2;
uint8_t gear : 2;
uint8_t reserve1 : 1;
uint8_t mode : 2;
uint8_t enabled : 1;
// Byte 1-2
// Byte 0-1
uint8_t reserve1;
uint8_t reserve2;
// Byte 2
uint8_t rpm;
// Byte 3
uint8_t gear;
// Byte 4
uint8_t work_enabled;
// Byte 5
uint8_t brake;
// Byte 6
uint8_t reserve3;
// Byte 3-4
uint8_t rpm_h;
uint8_t rpm_l;
// Byte 5-6
uint8_t brakeTime_h;
uint8_t brakeTime_l;
// Byte 7
uint8_t checksum;
uint8_t ctrl_enabled;
} fields;
uint8_t bytes[8];
@ -61,15 +61,15 @@ struct can_MCU_cmd
{
can_MCU_cmd_union data;
static constexpr uint32_t CMD_ID = 0x1A1;
static constexpr bool EXT_FLAG = false;
static constexpr uint32_t CMD_ID = 0x0CFF17EF;
static constexpr bool EXT_FLAG = true;
static constexpr bool RTR_FLAG = false;
// 构造函数自动初始化常量
can_MCU_cmd()
{
memset(&data, 0, sizeof(data));
data.fields.mode = 1;
data.fields.ctrl_enabled = 1;
data.fields.reserve1 = 0;
data.fields.reserve2 = 0;
data.fields.reserve3 = 0;
@ -77,40 +77,22 @@ struct can_MCU_cmd
void setEnabled(bool en)
{
data.fields.enabled = en ? 1 : 0;
data.fields.work_enabled = en ? 1 : 0;
}
void setGear(uint8_t gear)
{
data.fields.gear = gear & 0x03;
data.fields.gear = gear & 0x01;
}
void setBrake(uint8_t brake)
{
data.fields.brake = brake & 0x03;
data.fields.brake = brake & 0x01;
}
void setRPM(uint16_t rpm)
void setRPM(uint8_t rpm)
{
data.fields.rpm_h = (rpm >> 8) & 0xFF;
data.fields.rpm_l = rpm & 0xFF;
}
void setBrakeTime(uint16_t time_ms)
{
data.fields.brakeTime_h = (time_ms >> 8) & 0xFF;
data.fields.brakeTime_l = time_ms & 0xFF;
}
void pack()
{
// 计算校验：Byte0 ~ Byte6
uint8_t sum = 0;
for (int i = 0; i < 7; ++i)
{
sum += data.bytes[i];
}
data.fields.checksum = ~sum;
data.fields.rpm = std::clamp(rpm, static_cast<uint8_t>(0), static_cast<uint8_t>(100));
}
CANFrame toFrame() const
@ -136,16 +118,13 @@ struct can_EPS_cmd
can_EPS_cmd()
{
memset(&data, 0, sizeof(data));
data.fields.control_mode = 0x20;
}
void setControlMode(uint8_t mode)
void setAngle(float angle)
{
data.fields.control_mode = mode;
}
void setAngle(int16_t angle)
{
int16_t raw = angle + 1024; // angle公式：receive[3]*256+receive[4]-1024
// angle公式：receive[3]*256+receive[4]-1024
int16_t raw = static_cast<int16_t>(angle * 7.0f + 1024.0f);
data.fields.angle_h = (raw >> 8) & 0xFF;
data.fields.angle_l = raw & 0xFF;
}
@ -162,7 +141,7 @@ struct can_EPS_cmd
value = 20;
else if (value > 250)
value = 250;
data.fields.angular_speed = value;
data.fields.angular_speed = static_cast<uint8_t>(value);
}
void pack()
@ -194,11 +173,22 @@ struct can_VCU_out1_cmd
static constexpr bool EXT_FLAG = true;
static constexpr bool RTR_FLAG = false;
union Byte0
{
struct
{
uint8_t reserved1 : 3; // 保留位
uint8_t dust_shake : 1; // 振尘
uint8_t reserved2 : 4; // 保留位
} bits;
uint8_t raw = 0;
};
union Byte3
{
struct
{
uint8_t night_light : 4; // 低 4 位（夜行灯等级）
uint8_t headlight : 4; // 大灯
uint8_t reserved : 3; // 保留位
uint8_t leftLight : 1; // 左转灯
} bits;
@ -207,9 +197,12 @@ struct can_VCU_out1_cmd
uint8_t data[8]{};
Byte3 &byte3()
Byte0 &byte0() { return *reinterpret_cast<Byte0 *>(&data[0]); }
Byte3 &byte3() { return *reinterpret_cast<Byte3 *>(&data[3]); }
void setDustShake(bool on)
{
return *reinterpret_cast<Byte3 *>(&data[3]);
byte0().bits.dust_shake = on ? 1 : 0;
}
void setLeftLight(bool on)
@ -217,19 +210,9 @@ struct can_VCU_out1_cmd
byte3().bits.leftLight = on ? 1 : 0;
}
// 传入百分比（0~100），按每10%一级计算
void setNightLightPercent(uint8_t percent)
void setHeadLight(bool on)
{
if (percent > 100)
percent = 100;
uint8_t level = percent / 10; // 0–10
byte3().bits.night_light = level;
}
// 传入布尔值，true = 100%，false = 0%
void setNightLightPercent(bool on)
{
byte3().bits.night_light = on ? 0x0A : 0x00;
byte3().bits.headlight = on ? 0x64 : 0x00;
}
CANFrame toFrame() const
@ -266,7 +249,7 @@ struct can_VCU_out2_cmd
struct
{
uint8_t reserved1 : 5; // 保留位
uint8_t main_brush_lift : 1; // 主刷推杆
uint8_t sweep_ctrl : 1; // 扫地控制
uint8_t reserved2 : 1; // 保留位
uint8_t headlight : 1; // 大灯
} bits;
@ -280,9 +263,7 @@ struct can_VCU_out2_cmd
uint8_t edge_brush_lift : 1; // 边刷推杆
uint8_t reserved3 : 1; // 保留位
uint8_t spray : 1; // 喷水
uint8_t reserved4 : 1; // 保留位
uint8_t dust_shake : 1; // 振尘
uint8_t vacuum : 1; // 吸尘
uint8_t reserved4 : 3; // 保留位
uint8_t mud_flap : 1; // 挡皮
uint8_t reserved5 : 1; // 保留位
} bits;
@ -307,9 +288,9 @@ struct can_VCU_out2_cmd
}
// === Byte1 控制 ===
void setMainBrushLift(bool on)
void setSweeepCtrl(bool on)
{
byte1().bits.main_brush_lift = on ? 1 : 0;
byte1().bits.sweep_ctrl = on ? 1 : 0;
}
void setHeadlight(bool on)
@ -328,16 +309,6 @@ struct can_VCU_out2_cmd
byte3().bits.spray = on ? 1 : 0;
}
void setDustShake(bool on)
{
byte3().bits.dust_shake = on ? 1 : 0;
}
void setVacuum(bool on)
{
byte3().bits.vacuum = on ? 1 : 0;
}
void setMudFlap(bool on)
{
byte3().bits.mud_flap = on ? 1 : 0;
@ -355,157 +326,9 @@ struct can_VCU_out2_cmd
}
};
struct can_main_Brush_cmd
{
static constexpr uint32_t CMD_ID = 0x0cee36ef;
static constexpr bool EXT_FLAG = true;
static constexpr bool RTR_FLAG = false;
union Byte0
{
struct
{
uint8_t right_motor_pwm : 8;
} bits;
uint8_t raw = 0;
};
union Byte1
{
struct
{
uint8_t left_motor_pwm : 8;
} bits;
uint8_t raw = 0;
};
union Byte7
{
struct
{
uint8_t can_enabled : 8;
} bits;
uint8_t raw = 0;
};
uint8_t data[8]{};
can_main_Brush_cmd()
{
data[2] = 1; // 固定字节初始化
}
Byte0 &byte0() { return *reinterpret_cast<Byte0 *>(&data[0]); }
Byte1 &byte1() { return *reinterpret_cast<Byte1 *>(&data[1]); }
Byte7 &byte7() { return *reinterpret_cast<Byte7 *>(&data[7]); }
// === Byte0,1 控制 ===
void setBrushPwm(uint8_t pwm)
{
if (pwm > 100)
{
pwm = 100;
}
byte0().bits.right_motor_pwm = pwm;
byte1().bits.left_motor_pwm = pwm;
}
// === Byte7 控制 ===
void setBrushEnable(bool on)
{
byte7().bits.can_enabled = on ? 1 : 0;
}
CANFrame toFrame() const
{
CANFrame frame;
frame.id = CMD_ID;
std::copy(std::begin(data), std::end(data), frame.data);
frame.dlc = 8;
frame.ext = EXT_FLAG;
frame.rtr = RTR_FLAG;
return frame;
}
};
struct can_edge_Brush_cmd
{
static constexpr uint32_t CMD_ID = 0x0cee26ef;
static constexpr bool EXT_FLAG = true;
static constexpr bool RTR_FLAG = false;
union Byte0
{
struct
{
uint8_t right_motor_pwm : 8;
} bits;
uint8_t raw = 0;
};
union Byte1
{
struct
{
uint8_t left_motor_pwm : 8;
} bits;
uint8_t raw = 0;
};
union Byte7
{
struct
{
uint8_t can_enabled : 8;
} bits;
uint8_t raw = 0;
};
uint8_t data[8]{};
can_edge_Brush_cmd()
{
data[2] = 1; // 固定字节初始化
}
Byte0 &byte0() { return *reinterpret_cast<Byte0 *>(&data[0]); }
Byte1 &byte1() { return *reinterpret_cast<Byte1 *>(&data[1]); }
Byte7 &byte7() { return *reinterpret_cast<Byte7 *>(&data[7]); }
// === Byte0,1 控制 ===
void setBrushPwm(uint8_t pwm)
{
if (pwm > 100)
{
pwm = 100;
}
byte0().bits.right_motor_pwm = pwm;
byte1().bits.left_motor_pwm = pwm;
}
// === Byte7 控制 ===
void setBrushEnable(bool on)
{
byte7().bits.can_enabled = on ? 1 : 0;
}
CANFrame toFrame() const
{
CANFrame frame;
frame.id = CMD_ID;
std::copy(std::begin(data), std::end(data), frame.data);
frame.dlc = 8;
frame.ext = EXT_FLAG;
frame.rtr = RTR_FLAG;
return frame;
}
};
extern can_MCU_cmd mcu_cmd;
extern can_EPS_cmd eps_cmd;
extern can_VCU_out1_cmd vcu1_cmd;
extern can_VCU_out2_cmd vcu2_cmd;
extern can_main_Brush_cmd main_brush_cmd;
extern can_edge_Brush_cmd edge_brush_cmd;
#endif

31
src/mc/msg/McCtrl.msg
View File

@ -1,31 +0,0 @@
#清扫车控制指令
#mcu部分
bool mcu_enabled #是否使能
uint8 brake #电磁刹指令 0 放开 1 抱死
uint8 gear #挡位 0x00:void 0x01:R档 0x02:N档 0x03:D档
uint16 rpm #目标转速
uint16 brake_time_ms #刹车时间 500-30000ms
#eps部分
float32 angle #轮端转向角度 分辨率0.2° [-60.0,60.0] 适当缩减
uint16 angle_speed #转向角速度 120-1500rpm
#vcu部分
bool main_brush_lift #主刷推杆
bool edge_brush_lift #边刷推杆
bool vacuum #吸尘
bool spray #喷水
bool mud_flap #挡皮
bool dust_shake #振尘
bool left_light #左转向灯
bool right_light #右转向灯
bool night_light #夜行灯
bool brake_light #刹车灯
bool headlight #大灯
bool main_brush_spin #主刷旋转
bool edge_brush_spin #边刷旋转
uint8 main_brush_pwm #主刷转速
uint8 edge_brush_pwm #主刷转速

12
src/mc/package.xml
View File

@ -8,17 +8,9 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>rosidl_default_generators</build_depend>
<depend>sweeper_interfaces</depend>
<depend>rclcpp</depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<build_depend>builtin_interfaces</build_depend>
<exec_depend>builtin_interfaces</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<build_depend>rclcpp</build_depend>
<build_depend>std_msgs</build_depend>
<exec_depend>rclcpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<export>
<build_type>ament_cmake</build_type>

94
src/mc/src/can_driver.cpp
View File

@ -3,8 +3,12 @@
#include <stdexcept>
#include <iostream>
#include <thread>
#include <fcntl.h> // 用于 fcntl
#include <unistd.h> // 用于 close 和 read
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <linux/can.h>
#include <linux/can/raw.h>
CANDriver::CANDriver() = default;
@ -16,23 +20,19 @@ CANDriver::~CANDriver()
bool CANDriver::open(const std::string &interface)
{
if (running)
{
return false;
}
#ifdef _WIN32
std::cerr << "Windows implementation not available\n";
return false;
#else
// Linux SocketCAN实现
if ((sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0)
sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (sockfd < 0)
{
perror("socket");
return false;
}
struct ifreq ifr;
strcpy(ifr.ifr_name, interface.c_str());
std::strncpy(ifr.ifr_name, interface.c_str(), IFNAMSIZ - 1);
ifr.ifr_name[IFNAMSIZ - 1] = '\0';
if (ioctl(sockfd, SIOCGIFINDEX, &ifr) < 0)
{
perror("ioctl");
@ -40,51 +40,40 @@ bool CANDriver::open(const std::string &interface)
return false;
}
struct sockaddr_can addr;
struct sockaddr_can addr{};
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
if (bind(sockfd, (struct sockaddr *)&addr, sizeof(addr)) < 0)
if (bind(sockfd, reinterpret_cast<struct sockaddr *>(&addr), sizeof(addr)) < 0)
{
perror("bind");
::close(sockfd);
return false;
}
// 设置非阻塞模式
// 设置为非阻塞
int flags = fcntl(sockfd, F_GETFL, 0);
fcntl(sockfd, F_SETFL, flags | O_NONBLOCK); // 设置为非阻塞
fcntl(sockfd, F_SETFL, flags | O_NONBLOCK);
running = true;
receiveThread = std::thread(&CANDriver::receiveThreadFunc, this);
return true;
#endif
}
void CANDriver::close()
{
if (running)
{
if (!running)
return;
running = false;
if (receiveThread.joinable())
{
receiveThread.join();
}
#ifdef _WIN32
if (hCAN != INVALID_HANDLE_VALUE)
{
// CAN_Close(hCAN);
hCAN = INVALID_HANDLE_VALUE;
}
#else
if (sockfd >= 0)
{
::close(sockfd);
sockfd = -1;
}
#endif
}
}
bool CANDriver::sendFrame(const CANFrame &frame)
@ -92,32 +81,22 @@ bool CANDriver::sendFrame(const CANFrame &frame)
if (!running)
return false;
#ifdef _WIN32
// Windows发送实现
return false;
#else
struct can_frame raw_frame;
memset(&raw_frame, 0, sizeof(raw_frame));
struct can_frame raw_frame{};
raw_frame.can_id = frame.id;
if (frame.ext)
raw_frame.can_id |= CAN_EFF_FLAG;
if (frame.rtr)
raw_frame.can_id |= CAN_RTR_FLAG;
raw_frame.can_dlc = frame.dlc;
memcpy(raw_frame.data, frame.data, frame.dlc);
// 设置非阻塞写操作
int flags = fcntl(sockfd, F_GETFL, 0);
fcntl(sockfd, F_SETFL, flags | O_NONBLOCK); // 设置为非阻塞
std::memcpy(raw_frame.data, frame.data, frame.dlc);
if (write(sockfd, &raw_frame, sizeof(raw_frame)) != sizeof(raw_frame))
{
perror("write");
return false;
}
return true;
#endif
}
void CANDriver::setReceiveCallback(ReceiveCallback callback, void *userData)
@ -128,15 +107,11 @@ void CANDriver::setReceiveCallback(ReceiveCallback callback, void *userData)
bool CANDriver::setFilter(const std::vector<can_filter> &filters)
{
#ifdef _WIN32
return false;
#else
if (!running)
return false;
filters_ = filters; // 更新本地保存
filters_ = filters;
return applyFilters();
#endif
}
bool CANDriver::addFilter(const can_filter &filter)
@ -153,15 +128,11 @@ bool CANDriver::addFilters(const std::vector<can_filter> &filters)
bool CANDriver::applyFilters()
{
#ifdef _WIN32
return false;
#else
if (!running)
return false;
if (filters_.empty())
{
// 如果没有设置过滤器，就关闭硬件过滤器（接受所有帧）
setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FILTER, nullptr, 0);
return true;
}
@ -172,41 +143,32 @@ bool CANDriver::applyFilters()
perror("setsockopt");
return false;
}
return true;
#endif
}
void CANDriver::receiveThreadFunc()
{
#ifdef _WIN32
// Windows接收实现
#else
struct can_frame raw_frame;
while (running)
{
// 在非阻塞模式下读取数据
ssize_t nbytes = read(sockfd, &raw_frame, sizeof(raw_frame));
if (nbytes < 0)
{
if (errno != EAGAIN) // 非阻塞错误，忽略
{
if (errno != EAGAIN)
perror("read");
}
continue; // 没有数据时继续循环
continue;
}
if (nbytes == sizeof(raw_frame) && callback)
{
CANFrame frame;
frame.id = raw_frame.can_id & CAN_EFF_MASK;
frame.ext = !!(raw_frame.can_id & CAN_EFF_FLAG);
frame.rtr = !!(raw_frame.can_id & CAN_RTR_FLAG);
frame.ext = static_cast<bool>(raw_frame.can_id & CAN_EFF_FLAG);
frame.rtr = static_cast<bool>(raw_frame.can_id & CAN_RTR_FLAG);
frame.dlc = raw_frame.can_dlc;
memcpy(frame.data, raw_frame.data, raw_frame.can_dlc);
// 调用回调函数，处理接收到的帧
std::memcpy(frame.data, raw_frame.data, raw_frame.can_dlc);
callback(frame, userData);
}
}
#endif
}

2
src/mc/src/can_struct.cpp
View File

@ -4,5 +4,3 @@ can_MCU_cmd mcu_cmd;
can_EPS_cmd eps_cmd;
can_VCU_out1_cmd vcu1_cmd;
can_VCU_out2_cmd vcu2_cmd;
can_main_Brush_cmd main_brush_cmd;
can_edge_Brush_cmd edge_brush_cmd;

142
src/mc/src/mc.cpp
View File

@ -5,21 +5,22 @@
#include "mc/can_struct.h"
#include "mc/get_config.h"
#include "rclcpp/rclcpp.hpp"
#include "mc/msg/can_frame.hpp"
#include "mc/msg/mc_ctrl.hpp"
#include "sweeper_interfaces/msg/can_frame.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
namespace sweeperMsg = sweeper_interfaces::msg;
CANDriver canctl;
struct CanHandlerContext
{
rclcpp::Node::SharedPtr node;
std::shared_ptr<rclcpp::Publisher<mc::msg::CanFrame>> publisher;
std::shared_ptr<rclcpp::Publisher<sweeperMsg::CanFrame>> publisher;
};
std::atomic<bool> estop_active = false; // 急停状态量
std::atomic<bool> estop_msg_received = false; // 是否收到过急停帧
std::atomic<bool> vcu_msg_received = false; // 是否收到过vcu数据帧
std::atomic<bool> vcu_awake = false; // 是否唤醒成功
rclcpp::Time last_estop_msg_time; // 上次收到急停帧的时间
rclcpp::Time last_vcu_msg_time; // 上次收到vcu数据帧的时间
// 全局开关，控制是否打印 CAN 消息到终端
bool g_can_print_enable = false;
@ -27,11 +28,11 @@ bool g_can_print_enable = false;
struct ControlCache
{
std::mutex mutex; // 防止多线程冲突
mc::msg::McCtrl latest_msg;
sweeperMsg::McCtrl latest_msg;
std::chrono::steady_clock::time_point last_update_time;
bool has_data = false;
void update(const mc::msg::McCtrl &msg)
void update(const sweeperMsg::McCtrl &msg)
{
std::lock_guard<std::mutex> lock(mutex);
latest_msg = msg;
@ -39,7 +40,7 @@ struct ControlCache
has_data = true;
}
bool get(mc::msg::McCtrl &msg)
bool get(sweeperMsg::McCtrl &msg)
{
std::lock_guard<std::mutex> lock(mutex);
if (!has_data)
@ -61,27 +62,22 @@ struct ControlCache
ControlCache control_cache;
// 发布检测
mc::msg::McCtrl get_safe_control()
sweeperMsg::McCtrl get_safe_control()
{
mc::msg::McCtrl msg;
sweeperMsg::McCtrl msg;
if (!control_cache.get(msg))
{
// 超时或未接收到控制数据，进入安全状态
msg.mcu_enabled = true;
msg.brake = 1;
msg.gear = 0;
msg.rpm = 0;
msg.brake_time_ms = 500;
msg.angle = 0;
msg.angle_speed = 120;
msg.main_brush_lift = false;
msg.edge_brush_lift = false;
msg.vacuum = false;
msg.sweep_ctrl = false;
msg.spray = false;
msg.mud_flap = false;
msg.dust_shake = false;
msg.main_brush_spin = false;
msg.edge_brush_spin = false;
}
return msg;
}
@ -96,7 +92,7 @@ void receiveHandler(const CANFrame &frame, void *userData)
auto now = node->now();
// 创建并发布 CAN 消息
auto msg = mc::msg::CanFrame();
auto msg = sweeperMsg::CanFrame();
msg.id = frame.id;
msg.dlc = frame.dlc;
msg.data.assign(frame.data, frame.data + frame.dlc);
@ -121,61 +117,17 @@ void receiveHandler(const CANFrame &frame, void *userData)
RCLCPP_INFO(node->get_logger(), "%s", ss.str().c_str());
}
// 急停报文检查
if (frame.id == 0x18FA0121 && frame.dlc >= 3)
// VCU报文检查
if (frame.id == 0x18FA0121)
{
last_estop_msg_time = now; // 更新时间
estop_msg_received.store(true);
last_vcu_msg_time = now; // 更新时间
vcu_msg_received.store(true);
vcu_awake.store(true); // 唤醒成功
}
}
uint8_t estop_status = (frame.data[2] & 0x0c) >> 2; // 急停状态位
// 急停被按下
if (estop_status != 0x03)
void mcCtrlCallback(const sweeperMsg::McCtrl::SharedPtr msg)
{
if (!estop_active.load())
{
RCLCPP_WARN(node->get_logger(), "E-STOP TRIGGERED, entering safe mode");
}
estop_active.store(true);
// 立即更新控制缓存，强制进入安全状态
mc::msg::McCtrl safe_msg;
safe_msg.mcu_enabled = true;
safe_msg.brake = 1;
safe_msg.gear = 0;
safe_msg.rpm = 0;
safe_msg.brake_time_ms = 500;
safe_msg.angle = 0;
safe_msg.angle_speed = 120;
safe_msg.main_brush_lift = false;
safe_msg.edge_brush_lift = false;
safe_msg.vacuum = false;
safe_msg.spray = false;
safe_msg.mud_flap = false;
safe_msg.dust_shake = false;
safe_msg.main_brush_spin = false;
safe_msg.edge_brush_spin = false;
control_cache.update(safe_msg); // 用这个覆盖已有控制指令
}
else
{
if (estop_active.load())
{
RCLCPP_INFO(node->get_logger(), "E-STOP released, resuming control");
}
estop_active.store(false);
}
}
}
void mcCtrlCallback(const mc::msg::McCtrl::SharedPtr msg)
{
if (estop_active)
return;
control_cache.update(*msg);
}
@ -219,17 +171,17 @@ void setupTimers(rclcpp::Node::SharedPtr node)
VCUWakeUp();
} });
// 急停报文 watchdog 检查，200ms
// vcu报文 watchdog 检查，200ms
static rclcpp::TimerBase::SharedPtr timer_estop_watchdog = node->create_wall_timer(
std::chrono::milliseconds(200), [node]()
{
auto now = node->now();
auto elapsed = now - last_estop_msg_time;
auto elapsed = now - last_vcu_msg_time;
if (estop_msg_received.load() && elapsed > rclcpp::Duration::from_seconds(0.5))
if (vcu_msg_received.load() && elapsed > rclcpp::Duration::from_seconds(0.5))
{
RCLCPP_WARN(node->get_logger(), "[TIMER][VCU] Emergency stop message timeout, resetting wake-up state.");
estop_msg_received.store(false);
RCLCPP_WARN(node->get_logger(), "[TIMER][VCU] message timeout, resetting wake-up state.");
vcu_msg_received.store(false);
vcu_awake.store(false);
} });
@ -237,14 +189,11 @@ void setupTimers(rclcpp::Node::SharedPtr node)
static rclcpp::TimerBase::SharedPtr timer_mcu = node->create_wall_timer(
std::chrono::milliseconds(20), []()
{
mc::msg::McCtrl msg = get_safe_control();
sweeperMsg::McCtrl msg = get_safe_control();
mcu_cmd.setEnabled(msg.mcu_enabled);
mcu_cmd.setGear(msg.gear);
mcu_cmd.setRPM(msg.rpm);
mcu_cmd.setBrakeTime(msg.brake_time_ms);
mcu_cmd.setBrake(msg.brake);
mcu_cmd.pack();
canctl.sendFrame(mcu_cmd.toFrame()); });
@ -252,52 +201,35 @@ void setupTimers(rclcpp::Node::SharedPtr node)
static rclcpp::TimerBase::SharedPtr timer_eps = node->create_wall_timer(
std::chrono::milliseconds(50), []()
{
mc::msg::McCtrl msg = get_safe_control();
sweeperMsg::McCtrl msg = get_safe_control();
eps_cmd.setCenterCmd(0);
eps_cmd.setAngle(static_cast<int16_t>(msg.angle * 5));
eps_cmd.setAngle(msg.angle);
eps_cmd.setAngularSpeed(msg.angle_speed);
eps_cmd.setControlMode(0x20);
eps_cmd.pack();
canctl.sendFrame(eps_cmd.toFrame()); });
// VCU2 控制，10Hz
// VCU 控制，10Hz
static rclcpp::TimerBase::SharedPtr timer_vcu = node->create_wall_timer(
std::chrono::milliseconds(100), []()
{
mc::msg::McCtrl msg = get_safe_control();
sweeperMsg::McCtrl msg = get_safe_control();
vcu1_cmd.setLeftLight(msg.left_light);
vcu1_cmd.setNightLightPercent(msg.night_light);
vcu1_cmd.setDustShake(msg.dust_shake);
vcu1_cmd.setHeadLight(msg.headlight);
vcu2_cmd.setBrakeLight(msg.brake_light);
vcu2_cmd.setDustShake(msg.dust_shake);
vcu2_cmd.setEdgeBrushLift(msg.edge_brush_lift);
vcu2_cmd.setHeadlight(msg.headlight);
vcu2_cmd.setMainBrushLift(msg.main_brush_lift);
vcu2_cmd.setMudFlap(msg.mud_flap);
vcu2_cmd.setRightLight(msg.right_light);
vcu2_cmd.setSpray(msg.spray);
vcu2_cmd.setVacuum(msg.vacuum);
vcu2_cmd.setSweeepCtrl(msg.sweep_ctrl);
canctl.sendFrame(vcu1_cmd.toFrame());
canctl.sendFrame(vcu2_cmd.toFrame()); });
// 刷子控制，3.3Hz
static rclcpp::TimerBase::SharedPtr timer_brush = node->create_wall_timer(
std::chrono::milliseconds(300), []()
{
mc::msg::McCtrl msg = get_safe_control();
main_brush_cmd.setBrushEnable(msg.main_brush_spin);
main_brush_cmd.setBrushPwm(msg.main_brush_pwm);
edge_brush_cmd.setBrushEnable(msg.edge_brush_spin);
edge_brush_cmd.setBrushPwm(msg.edge_brush_pwm);
canctl.sendFrame(main_brush_cmd.toFrame());
canctl.sendFrame(edge_brush_cmd.toFrame()); });
}
int main(int argc, char **argv)
@ -310,16 +242,16 @@ int main(int argc, char **argv)
RCLCPP_INFO(node->get_logger(), "Starting mc package...");
// 创建一个 Publisher
auto can_publisher = node->create_publisher<mc::msg::CanFrame>("can_data", 10);
auto can_publisher = node->create_publisher<sweeperMsg::CanFrame>("can_data", 10);
// 创建 Subscriber（收控制指令）
auto subscriber = node->create_subscription<mc::msg::McCtrl>(
auto subscriber = node->create_subscription<sweeperMsg::McCtrl>(
"mc_ctrl", // 订阅的 topic 名字
10, // 队列长度
mcCtrlCallback // 收到消息后的回调
);
last_estop_msg_time = rclcpp::Time(0, 0, node->get_clock()->get_clock_type());
last_vcu_msg_time = rclcpp::Time(0, 0, node->get_clock()->get_clock_type());
Config mc_config;
load_config(mc_config);

4
src/mqtt_report/CMakeLists.txt
View File

@ -13,7 +13,7 @@ endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(mc REQUIRED)
find_package(sweeper_interfaces REQUIRED)
find_package(radio_ctrl REQUIRED)
find_package(std_msgs REQUIRED)
@ -21,7 +21,7 @@ file(GLOB MQTT_REPORT_SOURCES "src/*.cpp")
add_executable(mqtt_report_node ${MQTT_REPORT_SOURCES})
ament_target_dependencies(mqtt_report_node rclcpp mc std_msgs)
ament_target_dependencies(mqtt_report_node rclcpp sweeper_interfaces std_msgs)
# 虚拟头文件库，包含项目头文件路径
add_library(mqtt_headers INTERFACE)

2
src/mqtt_report/package.xml
View File

@ -8,7 +8,7 @@
<license>MIT</license>
<depend>rclcpp</depend>
<depend>mc</depend>
<depend>sweeper_interfaces</depend>
<depend>radio_ctrl</depend>
<buildtool_depend>ament_cmake</buildtool_depend>

12
src/mqtt_report/src/mqtt_report.cpp
View File

@ -1,11 +1,13 @@
#include <rclcpp/rclcpp.hpp>
#include "mc/msg/can_frame.hpp"
#include "sweeper_interfaces/msg/can_frame.hpp"
#include "mqtt_report/mqtt_client.hpp"
#include "mqtt_report/get_config.h"
#include "mqtt_report/report_struct.h"
#include "mqtt_report/fault_codes.h"
#include <algorithm>
namespace sweeperMsg = sweeper_interfaces::msg;
Config config; // 清扫车配置文件
GeneralMsg info_report; // 常规消息上报
@ -31,7 +33,7 @@ void updateWaterLevel(float newLevel)
}
// 解析can报文，做消息上报
void Msg_Handler(const mc::msg::CanFrame::SharedPtr msg)
void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg)
{
switch (msg->id)
{
@ -230,7 +232,7 @@ public:
RCLCPP_ERROR(this->get_logger(), "Failed to connect to MQTT server");
}
subscription_ = this->create_subscription<mc::msg::CanFrame>(
subscription_ = this->create_subscription<sweeperMsg::CanFrame>(
"can_data", 10, std::bind(&CanDataSubscriber::topic_callback, this, std::placeholders::_1));
info_timer_ = this->create_wall_timer(
@ -243,7 +245,7 @@ public:
}
private:
void topic_callback(const mc::msg::CanFrame::SharedPtr msg)
void topic_callback(const sweeperMsg::CanFrame::SharedPtr msg)
{
// RCLCPP_INFO(this->get_logger(), "Received CAN frame ID: %u, DLC: %u", msg->id, msg->dlc);
Msg_Handler(msg);
@ -265,7 +267,7 @@ private:
}
}
rclcpp::Subscription<mc::msg::CanFrame>::SharedPtr subscription_;
rclcpp::Subscription<sweeperMsg::CanFrame>::SharedPtr subscription_;
MQTTClientWrapper mqtt_client_;
std::string info_topic_;
std::string fault_topic_;

7
src/radio_ctrl/CMakeLists.txt
View File

@ -8,17 +8,16 @@ endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(mc REQUIRED)
find_package(std_msgs REQUIRED) # 若使用自定义消息，这里替换为对应 msg 包
find_package(sweeper_interfaces REQUIRED)
find_package(std_msgs REQUIRED)
find_package(ament_index_cpp REQUIRED)
# find_package(sensor_msgs REQUIRED) # 若用 Joy 消息就加这个
add_executable(radio_ctrl_node src/radio_ctrl.cpp src/uart_handler.cpp)
target_include_directories(
radio_ctrl_node PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
ament_target_dependencies(radio_ctrl_node rclcpp std_msgs mc)
ament_target_dependencies(radio_ctrl_node rclcpp std_msgs sweeper_interfaces)
install(TARGETS radio_ctrl_node DESTINATION lib/${PROJECT_NAME})

4
src/radio_ctrl/config/config.json
View File

@ -2,7 +2,7 @@
"serial_port": "/dev/ttyUSB0",
"baudrate": 100000,
"rmoctl_para": {
"mcu_rpm_max": 1000,
"eps_angle_max": 30.0
"mcu_rpm_max": 100,
"eps_angle_max": 50.0
}
}

4
src/radio_ctrl/package.xml
View File

@ -11,9 +11,7 @@
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<depend>mc</depend>
<!-- 如果用 Joy 消息 -->
<!-- <depend>sensor_msgs</depend> -->
<depend>sweeper_interfaces</depend>
<build_depend>ament_cmake</build_depend>
<exec_depend>ament_cmake</exec_depend>

64
src/radio_ctrl/src/radio_ctrl.cpp
View File

@ -3,8 +3,10 @@
#include <iostream>
#include <algorithm>
#include <rclcpp/rclcpp.hpp>
#include "mc/msg/mc_ctrl.hpp"
#include "mc/msg/can_frame.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
#include "sweeper_interfaces/msg/can_frame.hpp"
namespace sweeperMsg = sweeper_interfaces::msg;
constexpr float EPS_GEAR_RATIO = 16.5f;
constexpr float DELTA_T = 0.02f; // 20ms
@ -24,10 +26,10 @@ public:
}
// 发布控制指令消息的发布器
pub_ = this->create_publisher<mc::msg::McCtrl>("radio_mc_ctrl", 10);
pub_ = this->create_publisher<sweeperMsg::McCtrl>("radio_mc_ctrl", 10);
// 订阅CAN反馈的回调函数
can_sub_ = this->create_subscription<mc::msg::CanFrame>(
can_sub_ = this->create_subscription<sweeperMsg::CanFrame>(
"can_data", 10,
std::bind(&SBUSNode::can_callback, this, std::placeholders::_1));
@ -63,7 +65,7 @@ private:
bool data_safe = uart_handler_->get_data_safe(); // 获取数据安全性
auto msg = mc::msg::McCtrl(); // 控制消息对象
auto msg = sweeperMsg::McCtrl(); // 控制消息对象
uint16_t ch_data[8]; // 各通道遥控数据
if (data_safe) // 数据安全，进行数据解析并发布
@ -78,64 +80,45 @@ private:
if (ch_data[6] == 192) // 是否使能车辆控制
{
msg.mcu_enabled = true;
msg.brake = 0;
// 挡位选择
if (ch_data[7] == 192)
{
msg.gear = 3; // D挡
msg.gear = 0; // D挡
}
else if (ch_data[7] == 1792)
{
msg.gear = 1; // R挡
}
else if (ch_data[7] == 992)
{
msg.gear = 2; // N挡
}
else
{
msg.gear = 0; // 未知状态
}
// 油门 / 刹车逻辑
if (ch_data[1] <= speed[1])
{
msg.brake = 1;
msg.brake_time_ms = static_cast<uint16_t>(30000 + (500 - 30000) * (speed[1] - ch_data[1]) / (speed[1] - speed[0]));
msg.rpm = 0;
}
else
{
msg.brake = 0;
msg.brake_time_ms = 500;
msg.rpm = static_cast<uint16_t>(MCU_RPM_MAX * (ch_data[1] - speed[1]) / (speed[2] - speed[1]));
}
// 一键清扫
if (ch_data[5] == 1792)
{
msg.main_brush_lift = false;
msg.edge_brush_lift = false;
msg.vacuum = false;
msg.sweep_ctrl = false;
msg.spray = false;
msg.mud_flap = false;
msg.dust_shake = false;
msg.main_brush_spin = false;
msg.edge_brush_spin = false;
}
else if (ch_data[5] == 192)
{
msg.main_brush_lift = true;
msg.edge_brush_lift = true;
msg.vacuum = true;
msg.sweep_ctrl = true;
msg.spray = true;
msg.mud_flap = true;
msg.dust_shake = false;
msg.main_brush_spin = true;
msg.edge_brush_spin = true;
msg.main_brush_pwm = 100;
msg.edge_brush_pwm = 100;
}
// 转向逻辑
@ -158,49 +141,38 @@ private:
}
else // 未使能状态，发送安全默认控制指令
{
msg.mcu_enabled = true;
msg.brake = 1;
msg.gear = 0;
msg.rpm = 0;
msg.brake_time_ms = 500;
msg.angle = 0;
msg.angle_speed = 120;
msg.main_brush_lift = false;
msg.edge_brush_lift = false;
msg.vacuum = false;
msg.sweep_ctrl = false;
msg.spray = false;
msg.mud_flap = false;
msg.dust_shake = false;
msg.main_brush_spin = false;
msg.edge_brush_spin = false;
}
// 发布控制消息
pub_->publish(msg);
RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:"
<< "\n mcu_enabled: " << msg.mcu_enabled
<< "\n brake: " << static_cast<int>(msg.brake)
<< "\n gear: " << static_cast<int>(msg.gear)
<< "\n rpm: " << msg.rpm
<< "\n brake_time_ms: " << msg.brake_time_ms
<< "\n rpm: " << msg.ehb_anable
<< "\n angle: " << msg.ehb_brake_pressure
<< "\n angle: " << msg.angle
<< "\n angle_speed: " << msg.angle_speed
<< "\n main_brush_lift: " << msg.main_brush_lift
<< "\n edge_brush_lift: " << msg.edge_brush_lift
<< "\n vacuum: " << msg.vacuum
<< "\n vacuum: " << msg.sweep_ctrl
<< "\n spray: " << msg.spray
<< "\n mud_flap: " << msg.mud_flap
<< "\n dust_shake: " << msg.dust_shake
<< "\n left_light: " << msg.left_light
<< "\n right_light: " << msg.right_light
<< "\n night_light: " << msg.night_light
<< "\n brake_light: " << msg.brake_light
<< "\n headlight: " << msg.headlight
<< "\n main_brush_spin: " << msg.main_brush_spin
<< "\n edge_brush_spin: " << msg.edge_brush_spin
<< "\n main_brush_pwm: " << static_cast<int>(msg.main_brush_pwm)
<< "\n edge_brush_pwm: " << static_cast<int>(msg.edge_brush_pwm));
<< "\n headlight: " << msg.headlight);
}
else
{
@ -210,7 +182,7 @@ private:
}
// CAN反馈回调函数（用于获取当前转向角度）
void can_callback(const mc::msg::CanFrame::SharedPtr msg)
void can_callback(const sweeperMsg::CanFrame::SharedPtr msg)
{
// 判断是否为转向反馈帧（ID=0x401，且数据长度大于5）
if (msg->id == 0x401 && msg->dlc >= 5)
@ -224,8 +196,8 @@ private:
}
// ROS2 发布/订阅/定时器/串口读取器
rclcpp::Publisher<mc::msg::McCtrl>::SharedPtr pub_;
rclcpp::Subscription<mc::msg::CanFrame>::SharedPtr can_sub_;
rclcpp::Publisher<sweeperMsg::McCtrl>::SharedPtr pub_;
rclcpp::Subscription<sweeperMsg::CanFrame>::SharedPtr can_sub_;
rclcpp::TimerBase::SharedPtr timer_;
std::shared_ptr<UartHandler> uart_handler_;

39
src/sweeper_interfaces/CMakeLists.txt Normal file
View File

@ -0,0 +1,39 @@
cmake_minimum_required(VERSION 3.8)
project(sweeper_interfaces)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(std_msgs REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/McCtrl.msg"
"msg/CanFrame.msg"
DEPENDENCIES std_msgs
)
if(BUILD_TESTING)
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)
# 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)
ament_lint_auto_find_test_dependencies()
endif()
ament_export_dependencies(rosidl_default_runtime)
ament_export_dependencies(rosidl_default_generators)
install(
DIRECTORY msg
DESTINATION share/${PROJECT_NAME}
)
ament_package()

0
src/mc/msg/CanFrame.msg → src/sweeper_interfaces/msg/CanFrame.msg
View File

26
src/sweeper_interfaces/msg/McCtrl.msg Normal file
View File

@ -0,0 +1,26 @@
#清扫车控制指令
#mcu部分
uint8 brake #电磁刹指令 0 放开 1 抱死
uint8 gear #挡位 0x00:D档 0x01:R档
uint8 rpm #转速占空比 0-100
#ehb部分
bool ehb_anable #ehb使能
uint8 ehb_brake_pressure #ehb制动压力 0-8MPa
#eps部分
float32 angle #轮端转向角度 分辨率0.2° [-66.0,66.0] 适当缩减
uint16 angle_speed #转向角速度 120-1500rpm
#vcu部分
bool edge_brush_lift #边刷推杆
bool sweep_ctrl #扫地控制
bool spray #喷水
bool mud_flap #挡皮
bool dust_shake #振尘
bool left_light #左转向灯
bool right_light #右转向灯
bool brake_light #刹车灯
bool headlight #大灯

23
src/sweeper_interfaces/package.xml Normal file
View File

@ -0,0 +1,23 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>sweeper_interfaces</name>
<version>0.0.0</version>
<description>Custom message and service definitions for sweeper project.</description>
<maintainer email="zxwl@56.com">root</maintainer>
<license>MIT</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>