lyq/sweeper_whu
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

6003

Browse Source
This commit is contained in:
lyq 1970-01-02 12:46:42 +08:00
parent 5fda0b868a
commit 04cbb03e79
11 changed files with 109 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
config.json
View File

@ -1,13 +1,13 @@
{
"mqtt": {
"vid": "V060002",
"vid": "V060003",
"inter_net_address": "tcp://192.168.4.196",
"inter_net_port": 11883,
"external_net_address": "tcp://36.153.162.171",
"external_net_port": 19683,
"mqtt_user": "zxwl",
"mqtt_password": "zxwl1234@",
"pub_gps_topic": "/zxwl/vehicle/V060002/gps",
"remote_topic": "/zxwl/vehicle/V060002/ctrl"
"pub_gps_topic": "/zxwl/vehicle/V060003/gps",
"remote_topic": "/zxwl/vehicle/V060003/ctrl"
}
}

BIN
ros-foxy-pcl-conversions_2.2.1-1focal.20230606.034520_arm64.deb Normal file
View File

Binary file not shown.

44
run.sh
View File

@ -16,16 +16,34 @@ sleep 0.2s
}&
sleep 0.2s
# {
# gnome-terminal --title="report" --tab "XXD_ros" -- bash -c "ros2 run mqtt_report mqtt_report_node"
# }
# sleep 0.2s
# {
# gnome-terminal --title="rtk" --tab "XXD_ros" -- bash -c "ros2 run rtk rtk_node"
# }&
# sleep 0.2s
# {
# gnome-terminal --title="pub_gps" --tab "XXD_ros" -- bash -c "ros2 run pub_gps pub_gps_node"
# }
{
gnome-terminal --title="lidar" --tab "XXD_ros" -- bash -c "ros2 launch rslidar_sdk start_double.launch.py"
}&
sleep 0.1s
{
gnome-terminal --title="rtk" --tab "XXD_ros" -- bash -c "ros2 run rtk rtk_node"
}&
sleep 0.1s
{
gnome-terminal --title="route" --tab "XXD_ros" -- bash -c "ros2 run route route_node"
}&
sleep 0.1s
{
gnome-terminal --title="sub" --tab "XXD_ros" -- bash -c "ros2 run sub sub_node"
}&
sleep 0.1s
{
gnome-terminal --title="task_manager" --tab "XXD_ros" -- bash -c "ros2 run task_manager task_manager_node"
}&
sleep 0.1s
{
gnome-terminal --title="pl" --tab "XXD_ros" -- bash -c "ros2 run pl pl_node"
}&
sleep 0.1s
{
gnome-terminal --title="fu" --tab "XXD_ros" -- bash -c "ros2 launch fu fu.launch.py"
}
sleep 0.1s
{
gnome-terminal --title="pub_gps" --tab "XXD_ros" -- bash -c "ros2 run pub_gps pub_gps_node"
}

4
src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfig.cmake
View File

@ -19,8 +19,8 @@ if(${ENABLE_TRANSFORM})
add_definitions("-DENABLE_TRANSFORM")
endif(${ENABLE_TRANSFORM})
set(rs_driver_INCLUDE_DIRS "/home/alvin/project/zxwl/sweeper/200_whu/sweeper_whu/src/airy/rslidar_sdk-main/src/rs_driver/src;/usr/local/rslidar_sdk/include")
set(RS_DRIVER_INCLUDE_DIRS "/home/alvin/project/zxwl/sweeper/200_whu/sweeper_whu/src/airy/rslidar_sdk-main/src/rs_driver/src;/usr/local/rslidar_sdk/include")
set(rs_driver_INCLUDE_DIRS "/home/aiec/sweeper_whu/src/airy/rslidar_sdk-main/src/rs_driver/src;/usr/local/rslidar_sdk/include")
set(RS_DRIVER_INCLUDE_DIRS "/home/aiec/sweeper_whu/src/airy/rslidar_sdk-main/src/rs_driver/src;/usr/local/rslidar_sdk/include")
set(rs_driver_LIBRARIES "pthread;pcap")
set(RS_DRIVER_LIBRARIES "pthread;pcap")

2
src/fu/CMakeLists.txt
View File

@ -8,7 +8,7 @@ endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD 17)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")

2
src/fu/config/fu_params.yaml
View File

@ -4,7 +4,7 @@ FU:
enable_obstacle_stop: True
# 绕障功能 False or True
enable_obstacle_avoid: True
enable_obstacle_avoid: False
# 可视化栅格 False or True
enable_visualize_grid: False

7
src/fu/src/fu_node.cpp
View File

@ -674,12 +674,7 @@ private:
// 发布控制指令
sweeper_interfaces::msg::McCtrl message;
bool sweep = (avoid_ctx_.state == AVOID_NONE); // 避障状态为无避障 且 任务开始 才开启清扫
message.edge_brush_lift = sweep;
message.sweep_ctrl = sweep;
message.spray = sweep;
message.mud_flap = sweep;
message.dust_shake = sweep;
message.sweep = (avoid_ctx_.state == AVOID_NONE); // 避障状态为无避障 且 任务开始 才开启清扫
message.gear = 0;
message.brake = (publish_speed == 0) ? 1 : 0;
message.rpm = publish_speed;

39
src/sub/include/sub/sub_node.hpp
View File

@ -8,40 +8,11 @@ struct car_ctrl
// 1. 使能模式
int mode; // 0:手动驾驶, 3:远程驾驶
// 2. 挡位
int gear; // 0:P档, 1:D档, 2:R档
// 3. 油门，制动，转向
int throttle; // 0~65535，油门，松开为0，踩死为65535
int brake; // 0~65535，刹车，松开为0，踩死为65535
int steering; // 0~65535，转向，左打死为0，右打死为65535
// 4. 转向灯
int turnLight; // 0:关, 1:左转, 2:右转
// 5. 大灯
int headLight; // 0:关, 1:开
// 6. 主刷推杆
int mainBrush; // 0:关, 1:开
// 7. 扫地控制
int sweepCtrl; // 0:关, 1:开
// 8. 边刷推杆
int edgeBrush; // 0:关, 1:开
// 9. 挡皮控制
int flap; // 0:关, 1:开
// 10. 振尘控制
int dustShaking; // 0:关, 1:开
// 12. 水泵
int waterPump; // 0:关, 1:开
// 13. 倒灰
int dump; // 0:全部收回, 1:斗升, 2:倒斗
int gear; // 挡位 0：N档，1：D档，2：R档
int throttle; // 油门 0~65535
int steering; // 转向 0~65535
int brake; // 刹车 0~65535
int sweepCtrl; // 清扫 1:清扫 0:不清扫
};
extern car_ctrl car_ctrl_mes;

119
src/sub/src/sub_node.cpp
View File

@ -43,7 +43,7 @@ std::string mqtt_topic_remote;
// 新增：声明未定义的常量
std::string ADDRESS;
std::string CLIENTID_SUB = "sweeper_CLIENT_800"; // 客户端ID，提供默认值
std::string CLIENTID_SUB = "sweeper_whu_CLIENT_1"; // 客户端ID，提供默认值
std::chrono::steady_clock::time_point last_message_time = std::chrono::steady_clock::now();
constexpr auto QOS = 1;
@ -158,10 +158,6 @@ int msgarrvd(void *context, char *topicName, int topicLen, MQTTClient_message *m
car_ctrl_mes.mode = value;
else if (command == "gear")
car_ctrl_mes.gear = value;
else if (command == "turnLight")
car_ctrl_mes.turnLight = value;
else if (command == "headLight")
car_ctrl_mes.headLight = value;
else if (command == "drive")
{
std::string driveValues = data["value"].asString();
@ -178,28 +174,13 @@ int msgarrvd(void *context, char *topicName, int topicLen, MQTTClient_message *m
int brake = std::stoi(values[1]);
int steering = std::stoi(values[2]);
steering = 32768 - steering;
car_ctrl_mes.throttle = throttle / 65535.0 * 50;
car_ctrl_mes.throttle = throttle;
car_ctrl_mes.brake = brake;
car_ctrl_mes.steering = steering / 32768.0 * 40;
car_ctrl_mes.steering = steering;
}
}
// ===================== 使用 mainBrush 代 sweepCtrl，之后小程序适配了改回来 =====================
else if (command == "sweepCtrl")
// else if (command == "mainBrush")
car_ctrl_mes.sweepCtrl = value;
// ===================== 使用 mainBrush 代 sweepCtrl，之后小程序适配了改回来 =====================
else if (command == "edgeBrush")
car_ctrl_mes.edgeBrush = value;
else if (command == "flap")
car_ctrl_mes.flap = value;
else if (command == "dustShaking")
car_ctrl_mes.dustShaking = value;
else if (command == "waterPump")
car_ctrl_mes.waterPump = value;
else if (command == "dump")
car_ctrl_mes.dump = value;
else if (command == "gather")
get_route = value;
@ -352,16 +333,42 @@ private:
if (car_ctrl_mes.mode == 3) // 台架端远程驾驶模式
{
msg.brake = 0;
msg.gear = car_ctrl_mes.gear - 1;
msg.rpm = car_ctrl_mes.throttle;
// ehb部分
float brake_pressure = car_ctrl_mes.brake / 65535.0 * 8;
msg.ehb_anable = car_ctrl_mes.brake > 100 ? 1 : 0;
msg.ehb_brake_pressure = brake_pressure;
if (car_ctrl_mes.gear == 0) // N档
msg.gear = 0;
else if (car_ctrl_mes.gear == 1) // D档
msg.gear = 2;
else if (car_ctrl_mes.gear == 2) // R档
msg.gear = 1;
// eps部分
msg.angle = car_ctrl_mes.steering;
msg.rpm = car_ctrl_mes.throttle / 65535 * 1000;
// 转向
// 0~32200 -> -40~0 33200~65535 -> 0~40
int steering = 0;
if (car_ctrl_mes.steering < 32200)
{
const int originalWidth1 = 32200;
const int targetWidth1 = 40;
double unitLength1 = targetWidth1 / originalWidth1;
steering = -40 + static_cast<int>(car_ctrl_mes.steering * unitLength1);
}
else if (car_ctrl_mes.steering > 33200)
{
const int originalWidth2 = 32355; // 注意这里是从 33200 到 65535，共计 32355 个数
const int targetWidth2 = 40;
double unitLength2 = targetWidth2 / originalWidth2;
steering = static_cast<int>((car_ctrl_mes.steering - 32200) * unitLength2);
}
else
{
steering = 0; // 朝向前方
}
msg.angle = steering;
msg.angle_speed = 120;
}
else
@ -370,53 +377,25 @@ private:
msg.gear = 0;
msg.rpm = 0;
// ehb部分
msg.ehb_anable = 1;
msg.ehb_brake_pressure = 8;
// eps部分
msg.angle = 0;
msg.angle_speed = 120;
}
// vcu部分
// 非0即true 0:关, 1:开
msg.edge_brush_lift = (car_ctrl_mes.sweepCtrl != 0); // 和sweepCtrl<一键清扫>协同作业
msg.sweep_ctrl = (car_ctrl_mes.sweepCtrl != 0);
msg.spray = (car_ctrl_mes.waterPump != 0);
msg.mud_flap = (car_ctrl_mes.flap != 0);
msg.dust_shake = (car_ctrl_mes.dustShaking != 0);
msg.left_light = (car_ctrl_mes.turnLight == 1); // 1:左转时左灯亮
msg.right_light = (car_ctrl_mes.turnLight == 2); // 2:右转时右灯亮
msg.brake_light = msg.ehb_anable; // 0:关, 1:开
msg.headlight = car_ctrl_mes.headLight; // 0:关, 1:开
// 倒灰
// 0:全部收回, 1:斗升, 2:倒斗
msg.dump = car_ctrl_mes.dump;
msg.sweep = car_ctrl_mes.sweepCtrl;
pub_mc->publish(msg);
RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:"
<< "\n brake: " << static_cast<int>(msg.brake) // uint8
<< "\n gear: " << static_cast<int>(msg.gear) // uint8
<< "\n rpm: " << static_cast<int>(msg.rpm) // uint8
<< "\n ehb_enable: " << static_cast<int>(msg.ehb_anable) // bool
<< "\n ehb_brake_pressure: " << static_cast<float>(msg.ehb_brake_pressure) // float32
<< "\n angle: " << msg.angle // float32
<< "\n angle_speed: " << msg.angle_speed // uint16
<< "\n edge_brush_lift: " << static_cast<int>(msg.edge_brush_lift) // bool
<< "\n sweep_ctrl: " << static_cast<int>(msg.sweep_ctrl) // bool
<< "\n spray: " << static_cast<int>(msg.spray) // bool
<< "\n mud_flap: " << static_cast<int>(msg.mud_flap) // bool
<< "\n dust_shake: " << static_cast<int>(msg.dust_shake) // bool
<< "\n left_light: " << static_cast<int>(msg.left_light) // bool
<< "\n right_light: " << static_cast<int>(msg.right_light) // bool
<< "\n brake_light: " << static_cast<int>(msg.brake_light) // bool
<< "\n headlight: " << static_cast<int>(msg.headlight) // bool
<< "\n dump: " << static_cast<int>(msg.dump) // uint8
);
// RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:" << "\n 档位: " << [&]()
// {
// switch(msg.gear) {
// case 0: return "空挡";
// case 1: return "后退";
// case 2: return "前进";
// default: return "未知档位"; // 添加默认分支
// } }() << "\n 油门转速: " << msg.rpm << " rpm"
// << "\n 转向角度: " << msg.angle << "°"
// << "\n 刹车: " << (msg.brake == 0 ? "开(释放)" : "关(刹住)") << "\n 清扫: " << (msg.sweep ? "开启清扫" : "关闭清扫"));
}
}
@ -465,7 +444,7 @@ int main(int argc, char **argv)
{
init_main();
memset(&car_ctrl_mes, 0, sizeof(car_ctrl_mes));
car_ctrl_mes.steering = 32767.0;
car_ctrl_mes.steering = 32767;
pthread_create(&mqtt_sub_thread_t, NULL, mqtt_sub, NULL);
rclcpp::init(argc, argv);

6
src/sweeper_interfaces/CMakeLists.txt
View File

@ -13,7 +13,13 @@ find_package(std_msgs REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/McCtrl.msg"
"msg/CanFrame.msg"
"msg/Sub.msg"
"msg/Rtk.msg"
"msg/Pl.msg"
"msg/DetectLine.msg"
"msg/Route.msg"
"msg/Fu.msg"
"msg/Task.msg"
DEPENDENCIES std_msgs
)

10
start_all.sh Normal file → Executable file
View File

@ -2,10 +2,10 @@
set -e
# 加载 ROS2 系统环境
source /opt/ros/humble/setup.bash
source /opt/ros/foxy/setup.bash
# 加载你的工作空间环境
source /home/lyq/project/minibus/install/setup.bash
source ~/sweeper_whu/install/setup.bash
# 启动节点
ros2 run radio_ctrl radio_ctrl_node &
@ -14,5 +14,11 @@ ros2 run ctrl_arbiter ctrl_arbiter_node &
ros2 run mqtt_report mqtt_report_node &
ros2 run rtk rtk_node &
ros2 run pub_gps pub_gps_node &
ros2 launch rslidar_sdk start_double.launch.py &
ros2 run route route_node &
ros2 run sub sub_node &
ros2 run task_manager task_manager_node &
ros2 run pl pl_node &
ros2 launch fu fu.launch.py &
wait