From 698758baa737ac677071346a080867d6fe471a19 Mon Sep 17 00:00:00 2001 From: lyq Date: Sat, 25 Oct 2025 10:57:38 +0800 Subject: [PATCH] remote -> sub --- run.sh | 2 +- src/remote/include/remote/remote_node.hpp | 15 - src/remote/src/remote_node.cpp | 282 ------------- src/{remote => sub}/CMakeLists.txt | 33 +- .../include/paho_mqtt_3c/Base64.h | 0 .../include/paho_mqtt_3c/Clients.h | 0 .../include/paho_mqtt_3c/Heap.h | 0 .../include/paho_mqtt_3c/LinkedList.h | 0 .../include/paho_mqtt_3c/Log.h | 0 .../include/paho_mqtt_3c/MQTTAsync.h | 0 .../include/paho_mqtt_3c/MQTTClient.h | 0 .../paho_mqtt_3c/MQTTClientPersistence.h | 0 .../include/paho_mqtt_3c/MQTTConnect.h | 0 .../include/paho_mqtt_3c/MQTTFormat.h | 0 .../include/paho_mqtt_3c/MQTTPacket.h | 0 .../include/paho_mqtt_3c/MQTTPacketOut.h | 0 .../include/paho_mqtt_3c/MQTTPersistence.h | 0 .../paho_mqtt_3c/MQTTPersistenceDefault.h | 0 .../include/paho_mqtt_3c/MQTTProperties.h | 0 .../include/paho_mqtt_3c/MQTTProtocol.h | 0 .../include/paho_mqtt_3c/MQTTProtocolClient.h | 0 .../include/paho_mqtt_3c/MQTTProtocolOut.h | 0 .../include/paho_mqtt_3c/MQTTPublish.h | 0 .../include/paho_mqtt_3c/MQTTReasonCodes.h | 0 .../include/paho_mqtt_3c/MQTTSubscribe.h | 0 .../include/paho_mqtt_3c/MQTTSubscribeOpts.h | 0 .../include/paho_mqtt_3c/MQTTUnsubscribe.h | 0 .../include/paho_mqtt_3c/Messages.h | 0 .../include/paho_mqtt_3c/OsWrapper.h | 0 .../include/paho_mqtt_3c/SHA1.h | 0 .../include/paho_mqtt_3c/SSLSocket.h | 0 .../include/paho_mqtt_3c/Socket.h | 0 .../include/paho_mqtt_3c/SocketBuffer.h | 0 .../include/paho_mqtt_3c/StackTrace.h | 0 .../include/paho_mqtt_3c/Thread.h | 0 .../include/paho_mqtt_3c/Tree.h | 0 .../include/paho_mqtt_3c/VersionInfo.h.in | 0 .../include/paho_mqtt_3c/WebSocket.h | 0 .../include/paho_mqtt_3c/mutex_type.h | 0 .../include/paho_mqtt_3c/pubsub_opts.h | 0 .../include/paho_mqtt_3c/utf-8.h | 0 .../include/remote => sub/include/sub}/json.h | 0 src/sub/include/sub/md5.h | 56 +++ src/sub/include/sub/sub_node.hpp | 20 + .../lib/libpaho-mqtt3c-static.a | Bin src/{remote => sub}/lib/libpaho-mqtt3c.a | Bin src/{remote => sub}/package.xml | 4 +- src/{remote => sub}/src/jsoncpp.cpp | 0 src/sub/src/md5.cpp | 371 ++++++++++++++++++ 49 files changed, 468 insertions(+), 315 deletions(-) delete mode 100644 src/remote/include/remote/remote_node.hpp delete mode 100644 src/remote/src/remote_node.cpp rename src/{remote => sub}/CMakeLists.txt (79%) rename src/{remote => sub}/include/paho_mqtt_3c/Base64.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/Clients.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/Heap.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/LinkedList.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/Log.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/MQTTAsync.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/MQTTClient.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/MQTTClientPersistence.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/MQTTConnect.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/MQTTFormat.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/MQTTPacket.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/MQTTPacketOut.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/MQTTPersistence.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/MQTTPersistenceDefault.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/MQTTProperties.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/MQTTProtocol.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/MQTTProtocolClient.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/MQTTProtocolOut.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/MQTTPublish.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/MQTTReasonCodes.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/MQTTSubscribe.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/MQTTSubscribeOpts.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/MQTTUnsubscribe.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/Messages.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/OsWrapper.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/SHA1.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/SSLSocket.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/Socket.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/SocketBuffer.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/StackTrace.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/Thread.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/Tree.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/VersionInfo.h.in (100%) rename src/{remote => sub}/include/paho_mqtt_3c/WebSocket.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/mutex_type.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/pubsub_opts.h (100%) rename src/{remote => sub}/include/paho_mqtt_3c/utf-8.h (100%) rename src/{remote/include/remote => sub/include/sub}/json.h (100%) create mode 100644 src/sub/include/sub/md5.h create mode 100644 src/sub/include/sub/sub_node.hpp rename src/{remote => sub}/lib/libpaho-mqtt3c-static.a (100%) rename src/{remote => sub}/lib/libpaho-mqtt3c.a (100%) rename src/{remote => sub}/package.xml (88%) rename src/{remote => sub}/src/jsoncpp.cpp (100%) create mode 100644 src/sub/src/md5.cpp diff --git a/run.sh b/run.sh index 45b7a20..526443f 100755 --- a/run.sh +++ b/run.sh @@ -34,7 +34,7 @@ sleep 0.1s }& sleep 0.1s { - gnome-terminal --title="remote" --tab "XXD_ros" -- bash -c "ros2 run remote remote_node" + gnome-terminal --title="sub" --tab "XXD_ros" -- bash -c "ros2 run sub sub_node" }& sleep 0.1s { diff --git a/src/remote/include/remote/remote_node.hpp b/src/remote/include/remote/remote_node.hpp deleted file mode 100644 index 9a6a1e3..0000000 --- a/src/remote/include/remote/remote_node.hpp +++ /dev/null @@ -1,15 +0,0 @@ -#ifndef __REMOTE_NODE_H__ -#define __REMOTE_NODE_H__ - -struct alignas(8) car_ctrl -{ - int gear; // 挡位 0：N档，1：D档，2：R档 - float throttle; // 油门 0~65535 - float steering; // 转向 0~65535 - float brake; // 刹车 0~65535 - int sweep; // 清扫 1:清扫 0:不清扫 -}; - -extern car_ctrl car_ctrl_mes; - -#endif \ No newline at end of file diff --git a/src/remote/src/remote_node.cpp b/src/remote/src/remote_node.cpp deleted file mode 100644 index 445cbea..0000000 --- a/src/remote/src/remote_node.cpp +++ /dev/null @@ -1,282 +0,0 @@ -#include "MQTTClient.h" -#include "rclcpp/rclcpp.hpp" -#include "sweeper_interfaces/msg/mc_ctrl.hpp" -#include "json.h" -#include "remote_node.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace sweeperMsg = sweeper_interfaces::msg; - -Json::Value root; -Json::Reader reader; -Json::Value data; -std::string command; -std::string remote_topic; -int time_cunt = 0; -constexpr auto ADDRESS = "tcp://36.153.162.171:19683"; // 更改此处地址 -// constexpr auto ADDRESS = "tcp://192.168.4.196:11883"; // 更改此处地址 -constexpr auto CLIENTID_REMOTE = "SWEEPER_CLIENT_REMOTE_1"; // 更改此处客户端ID - -const char *getRemoteTopic() -{ - return remote_topic.c_str(); -} - -constexpr auto QOS = 1; -constexpr auto TIMEOUT = 10000L; - -volatile MQTTClient_deliveryToken deliveredtoken; - -char remote_buff[500]; - -car_ctrl car_ctrl_mes; - -void delivered(void *context, MQTTClient_deliveryToken dt) -{ - (void)context; - printf("Message with token value %d delivery confirmed\n", dt); - deliveredtoken = dt; -} - -int msgarrvd(void *context, char *topicName, int topicLen, MQTTClient_message *message) -{ - (void)context; - (void)topicLen; - time_cunt = 0; - printf("Message arrived\n"); - printf("topic: %s\n", topicName); - printf("message: %.*s\n", message->payloadlen, (char *)message->payload); - - memset(remote_buff, 0, sizeof(remote_buff)); - memcpy(&remote_buff, (char *)message->payload, message->payloadlen); - - root.clear(); - - if (!reader.parse(remote_buff, root)) - { - printf("recv json fail\n"); - return 1; - } - else - { - std::cout << root << std::endl; - - data = root["data"]; - command = root["command"].asString(); - - if (command.compare("gear") == 0) - { - car_ctrl_mes.gear = root["value"].asInt(); - } - else if (command.compare("throttle") == 0) - { - car_ctrl_mes.throttle = root["value"].asFloat(); - } - else if (command.compare("steering") == 0) - { - car_ctrl_mes.steering = root["value"].asFloat(); - } - else if (command.compare("brake") == 0) - { - car_ctrl_mes.brake = root["value"].asFloat(); - } - else if (command.compare("sweep") == 0) - { - car_ctrl_mes.sweep = root["value"].asInt(); - } - } - - MQTTClient_freeMessage(&message); - MQTTClient_free(topicName); - return 1; -} - -void connlost(void *context, char *cause) -{ - (void)context; - printf("\nConnection lost\n"); - printf("cause: %s\n", cause); -} - -void *mqtt_remote(void *arg) -{ - (void)arg; - MQTTClient client; - // MQTTClient_deliveryToken token_d_m; - - MQTTClient_connectOptions conn_opts = MQTTClient_connectOptions_initializer; - // MQTTClient_message pubmsg_d_m = MQTTClient_message_initializer; - const char *REMOTE_TOPIC = getRemoteTopic(); - - int rc; - char *username = "zxwl"; - char *password = "zxwl1234@"; - - if ((rc = MQTTClient_create(&client, ADDRESS, CLIENTID_REMOTE, MQTTCLIENT_PERSISTENCE_NONE, NULL)) != MQTTCLIENT_SUCCESS) - { - printf("Failed to create client, return code %d\n", rc); - MQTTClient_destroy(&client); - return NULL; - } - - if ((rc = MQTTClient_setCallbacks(client, NULL, connlost, msgarrvd, delivered)) != MQTTCLIENT_SUCCESS) - { - printf("Failed to set callbacks, return code %d\n", rc); - MQTTClient_destroy(&client); - return NULL; - } - - conn_opts.keepAliveInterval = 20; - conn_opts.cleansession = 1; - conn_opts.username = username; - conn_opts.password = password; - - if ((rc = MQTTClient_connect(client, &conn_opts)) != MQTTCLIENT_SUCCESS) - { - printf("Failed to connect, return code %d\n", rc); - MQTTClient_destroy(&client); - return NULL; - } - - printf("Subscribing to topic %s\nfor client %s using QoS%d\n\n", REMOTE_TOPIC, CLIENTID_REMOTE, QOS); - if ((rc = MQTTClient_subscribe(client, REMOTE_TOPIC, QOS)) != MQTTCLIENT_SUCCESS) - { - printf("Failed to subscribe, return code %d\n", rc); - MQTTClient_destroy(&client); - return NULL; - } - - while (1) - { - usleep(200000); - } - - if ((rc = MQTTClient_unsubscribe(client, REMOTE_TOPIC)) != MQTTCLIENT_SUCCESS) - { - printf("Failed to unsubscribe, return code %d\n", rc); - MQTTClient_destroy(&client); - return NULL; - } - - if ((rc = MQTTClient_disconnect(client, 10000)) != MQTTCLIENT_SUCCESS) - { - printf("Failed to disconnect, return code %d\n", rc); - MQTTClient_destroy(&client); - return NULL; - } - - MQTTClient_destroy(&client); - return NULL; -} - -class remote_node : public rclcpp::Node -{ -public: - // 构造函数,有一个参数为节点名称 - remote_node(std::string name) : Node(name) - { - RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str()); - msg_publisher_ = this->create_publisher("ctrl_command", 10); - timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&remote_node::timer_callback, this)); - } - -private: - void timer_callback() - { - sweeperMsg::McCtrl message; - message.gear = car_ctrl_mes.gear; - if (car_ctrl_mes.gear == 0) // N档 - message.gear = 0; - else if (car_ctrl_mes.gear == 1) // D档 - message.gear = 2; - else if (car_ctrl_mes.gear == 2) // R档 - message.gear = 1; - - // 刹车 - message.brake = (car_ctrl_mes.brake > 0) ? true : false; - - // 油门 映射到 [0, 1000] 范围 - message.rpm = car_ctrl_mes.throttle / 65535.0f * 1000.0f; - - // 转向 - // 0~32200 -> -40~0 33200~65535 -> 0~40 - float steering = 0; - if (car_ctrl_mes.steering < 32200) - { - const double originalWidth1 = 32200.0; - const double targetWidth1 = 40.0; - - double unitLength1 = targetWidth1 / originalWidth1; - - steering = -40 + static_cast(car_ctrl_mes.steering * unitLength1); - } - else if (car_ctrl_mes.steering > 33200) - { - const double originalWidth2 = 32355.0; // 注意这里是从 33200 到 65535，共计 32355 个数 - const double targetWidth2 = 40.0; - - double unitLength2 = targetWidth2 / originalWidth2; - - steering = static_cast((car_ctrl_mes.steering - 32200) * unitLength2); - } - else - { - steering = 0.0; // 朝向前方 - } - message.angle = steering; - - message.sweep = car_ctrl_mes.sweep; - - msg_publisher_->publish(message); - } - - // 声名定时器指针 - rclcpp::TimerBase::SharedPtr timer_; - // 声明话题发布者指针 - rclcpp::Publisher::SharedPtr msg_publisher_; -}; - -void init_main() -{ - Json::Reader reader; - Json::Value root; - std::ifstream in("config.json", std::ios::binary); - if (!in.is_open()) - { - std::cout << "read config file error" << std::endl; - return; - } - if (reader.parse(in, root)) - { - remote_topic = root["mqtt"]["remote_topic"].asString(); - } -} - -pthread_t mqtt_remote_thread_t; - -int main(int argc, char **argv) -{ - init_main(); - memset(&car_ctrl_mes, 0, sizeof(car_ctrl_mes)); - pthread_create(&mqtt_remote_thread_t, NULL, mqtt_remote, NULL); - - rclcpp::init(argc, argv); - /*创建对应节点的共享指针对象*/ - auto node = std::make_shared("remote_node"); - /* 运行节点，并检测退出信号*/ - rclcpp::spin(node); - rclcpp::shutdown(); - - return 0; -} \ No newline at end of file diff --git a/src/remote/CMakeLists.txt b/src/sub/CMakeLists.txt similarity index 79% rename from src/remote/CMakeLists.txt rename to src/sub/CMakeLists.txt index d4e264f..1062f27 100644 --- a/src/remote/CMakeLists.txt +++ b/src/sub/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(remote) +project(sub) # Default to C99 if(NOT CMAKE_C_STANDARD) @@ -24,33 +24,37 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(sweeper_interfaces REQUIRED) +find_package(CURL REQUIRED) include_directories( - include/remote + include/sub include/paho_mqtt_3c + ${catkin_INCLUDE_DIRS} ) -add_executable(remote_node - src/remote_node.cpp +add_executable(sub_node + src/sub_node.cpp src/jsoncpp.cpp + src/md5.cpp ) -ament_target_dependencies(remote_node rclcpp std_msgs sweeper_interfaces) +ament_target_dependencies(sub_node rclcpp std_msgs sweeper_interfaces CURL) -if(CMAKE_SYSTEM_PROCESSOR MATCHES aarch64) + +if(CMAKE_SYSTEM_PROCESSOR MATCHES aarch64) target_link_libraries( - remote_node + sub_node ${PROJECT_SOURCE_DIR}/lib/libpaho-mqtt3c-static.a ) -else() +else() target_link_libraries( - remote_node + sub_node ${PROJECT_SOURCE_DIR}/lib/libpaho-mqtt3c.a - ) -endif() + ) +endif() install(TARGETS - remote_node + sub_node DESTINATION lib/${PROJECT_NAME} ) @@ -63,6 +67,5 @@ if(BUILD_TESTING) # uncomment the line when this package is not in a git repo #set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() -endif() - -ament_package() +endif() +ament_package() \ No newline at end of file diff --git a/src/remote/include/paho_mqtt_3c/Base64.h b/src/sub/include/paho_mqtt_3c/Base64.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/Base64.h rename to src/sub/include/paho_mqtt_3c/Base64.h diff --git a/src/remote/include/paho_mqtt_3c/Clients.h b/src/sub/include/paho_mqtt_3c/Clients.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/Clients.h rename to src/sub/include/paho_mqtt_3c/Clients.h diff --git a/src/remote/include/paho_mqtt_3c/Heap.h b/src/sub/include/paho_mqtt_3c/Heap.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/Heap.h rename to src/sub/include/paho_mqtt_3c/Heap.h diff --git a/src/remote/include/paho_mqtt_3c/LinkedList.h b/src/sub/include/paho_mqtt_3c/LinkedList.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/LinkedList.h rename to src/sub/include/paho_mqtt_3c/LinkedList.h diff --git a/src/remote/include/paho_mqtt_3c/Log.h b/src/sub/include/paho_mqtt_3c/Log.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/Log.h rename to src/sub/include/paho_mqtt_3c/Log.h diff --git a/src/remote/include/paho_mqtt_3c/MQTTAsync.h b/src/sub/include/paho_mqtt_3c/MQTTAsync.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/MQTTAsync.h rename to src/sub/include/paho_mqtt_3c/MQTTAsync.h diff --git a/src/remote/include/paho_mqtt_3c/MQTTClient.h b/src/sub/include/paho_mqtt_3c/MQTTClient.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/MQTTClient.h rename to src/sub/include/paho_mqtt_3c/MQTTClient.h diff --git a/src/remote/include/paho_mqtt_3c/MQTTClientPersistence.h b/src/sub/include/paho_mqtt_3c/MQTTClientPersistence.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/MQTTClientPersistence.h rename to src/sub/include/paho_mqtt_3c/MQTTClientPersistence.h diff --git a/src/remote/include/paho_mqtt_3c/MQTTConnect.h b/src/sub/include/paho_mqtt_3c/MQTTConnect.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/MQTTConnect.h rename to src/sub/include/paho_mqtt_3c/MQTTConnect.h diff --git a/src/remote/include/paho_mqtt_3c/MQTTFormat.h b/src/sub/include/paho_mqtt_3c/MQTTFormat.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/MQTTFormat.h rename to src/sub/include/paho_mqtt_3c/MQTTFormat.h diff --git a/src/remote/include/paho_mqtt_3c/MQTTPacket.h b/src/sub/include/paho_mqtt_3c/MQTTPacket.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/MQTTPacket.h rename to src/sub/include/paho_mqtt_3c/MQTTPacket.h diff --git a/src/remote/include/paho_mqtt_3c/MQTTPacketOut.h b/src/sub/include/paho_mqtt_3c/MQTTPacketOut.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/MQTTPacketOut.h rename to src/sub/include/paho_mqtt_3c/MQTTPacketOut.h diff --git a/src/remote/include/paho_mqtt_3c/MQTTPersistence.h b/src/sub/include/paho_mqtt_3c/MQTTPersistence.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/MQTTPersistence.h rename to src/sub/include/paho_mqtt_3c/MQTTPersistence.h diff --git a/src/remote/include/paho_mqtt_3c/MQTTPersistenceDefault.h b/src/sub/include/paho_mqtt_3c/MQTTPersistenceDefault.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/MQTTPersistenceDefault.h rename to src/sub/include/paho_mqtt_3c/MQTTPersistenceDefault.h diff --git a/src/remote/include/paho_mqtt_3c/MQTTProperties.h b/src/sub/include/paho_mqtt_3c/MQTTProperties.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/MQTTProperties.h rename to src/sub/include/paho_mqtt_3c/MQTTProperties.h diff --git a/src/remote/include/paho_mqtt_3c/MQTTProtocol.h b/src/sub/include/paho_mqtt_3c/MQTTProtocol.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/MQTTProtocol.h rename to src/sub/include/paho_mqtt_3c/MQTTProtocol.h diff --git a/src/remote/include/paho_mqtt_3c/MQTTProtocolClient.h b/src/sub/include/paho_mqtt_3c/MQTTProtocolClient.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/MQTTProtocolClient.h rename to src/sub/include/paho_mqtt_3c/MQTTProtocolClient.h diff --git a/src/remote/include/paho_mqtt_3c/MQTTProtocolOut.h b/src/sub/include/paho_mqtt_3c/MQTTProtocolOut.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/MQTTProtocolOut.h rename to src/sub/include/paho_mqtt_3c/MQTTProtocolOut.h diff --git a/src/remote/include/paho_mqtt_3c/MQTTPublish.h b/src/sub/include/paho_mqtt_3c/MQTTPublish.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/MQTTPublish.h rename to src/sub/include/paho_mqtt_3c/MQTTPublish.h diff --git a/src/remote/include/paho_mqtt_3c/MQTTReasonCodes.h b/src/sub/include/paho_mqtt_3c/MQTTReasonCodes.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/MQTTReasonCodes.h rename to src/sub/include/paho_mqtt_3c/MQTTReasonCodes.h diff --git a/src/remote/include/paho_mqtt_3c/MQTTSubscribe.h b/src/sub/include/paho_mqtt_3c/MQTTSubscribe.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/MQTTSubscribe.h rename to src/sub/include/paho_mqtt_3c/MQTTSubscribe.h diff --git a/src/remote/include/paho_mqtt_3c/MQTTSubscribeOpts.h b/src/sub/include/paho_mqtt_3c/MQTTSubscribeOpts.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/MQTTSubscribeOpts.h rename to src/sub/include/paho_mqtt_3c/MQTTSubscribeOpts.h diff --git a/src/remote/include/paho_mqtt_3c/MQTTUnsubscribe.h b/src/sub/include/paho_mqtt_3c/MQTTUnsubscribe.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/MQTTUnsubscribe.h rename to src/sub/include/paho_mqtt_3c/MQTTUnsubscribe.h diff --git a/src/remote/include/paho_mqtt_3c/Messages.h b/src/sub/include/paho_mqtt_3c/Messages.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/Messages.h rename to src/sub/include/paho_mqtt_3c/Messages.h diff --git a/src/remote/include/paho_mqtt_3c/OsWrapper.h b/src/sub/include/paho_mqtt_3c/OsWrapper.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/OsWrapper.h rename to src/sub/include/paho_mqtt_3c/OsWrapper.h diff --git a/src/remote/include/paho_mqtt_3c/SHA1.h b/src/sub/include/paho_mqtt_3c/SHA1.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/SHA1.h rename to src/sub/include/paho_mqtt_3c/SHA1.h diff --git a/src/remote/include/paho_mqtt_3c/SSLSocket.h b/src/sub/include/paho_mqtt_3c/SSLSocket.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/SSLSocket.h rename to src/sub/include/paho_mqtt_3c/SSLSocket.h diff --git a/src/remote/include/paho_mqtt_3c/Socket.h b/src/sub/include/paho_mqtt_3c/Socket.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/Socket.h rename to src/sub/include/paho_mqtt_3c/Socket.h diff --git a/src/remote/include/paho_mqtt_3c/SocketBuffer.h b/src/sub/include/paho_mqtt_3c/SocketBuffer.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/SocketBuffer.h rename to src/sub/include/paho_mqtt_3c/SocketBuffer.h diff --git a/src/remote/include/paho_mqtt_3c/StackTrace.h b/src/sub/include/paho_mqtt_3c/StackTrace.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/StackTrace.h rename to src/sub/include/paho_mqtt_3c/StackTrace.h diff --git a/src/remote/include/paho_mqtt_3c/Thread.h b/src/sub/include/paho_mqtt_3c/Thread.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/Thread.h rename to src/sub/include/paho_mqtt_3c/Thread.h diff --git a/src/remote/include/paho_mqtt_3c/Tree.h b/src/sub/include/paho_mqtt_3c/Tree.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/Tree.h rename to src/sub/include/paho_mqtt_3c/Tree.h diff --git a/src/remote/include/paho_mqtt_3c/VersionInfo.h.in b/src/sub/include/paho_mqtt_3c/VersionInfo.h.in similarity index 100% rename from src/remote/include/paho_mqtt_3c/VersionInfo.h.in rename to src/sub/include/paho_mqtt_3c/VersionInfo.h.in diff --git a/src/remote/include/paho_mqtt_3c/WebSocket.h b/src/sub/include/paho_mqtt_3c/WebSocket.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/WebSocket.h rename to src/sub/include/paho_mqtt_3c/WebSocket.h diff --git a/src/remote/include/paho_mqtt_3c/mutex_type.h b/src/sub/include/paho_mqtt_3c/mutex_type.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/mutex_type.h rename to src/sub/include/paho_mqtt_3c/mutex_type.h diff --git a/src/remote/include/paho_mqtt_3c/pubsub_opts.h b/src/sub/include/paho_mqtt_3c/pubsub_opts.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/pubsub_opts.h rename to src/sub/include/paho_mqtt_3c/pubsub_opts.h diff --git a/src/remote/include/paho_mqtt_3c/utf-8.h b/src/sub/include/paho_mqtt_3c/utf-8.h similarity index 100% rename from src/remote/include/paho_mqtt_3c/utf-8.h rename to src/sub/include/paho_mqtt_3c/utf-8.h diff --git a/src/remote/include/remote/json.h b/src/sub/include/sub/json.h similarity index 100% rename from src/remote/include/remote/json.h rename to src/sub/include/sub/json.h diff --git a/src/sub/include/sub/md5.h b/src/sub/include/sub/md5.h new file mode 100644 index 0000000..df10a8c --- /dev/null +++ b/src/sub/include/sub/md5.h @@ -0,0 +1,56 @@ +#ifndef MD5_H +#define MD5_H + +#include +#include + +/* Type define */ +typedef unsigned char byte; +typedef unsigned int uint32; + +using std::ifstream; +using std::string; + +/* MD5 declaration. */ +class MD5 +{ +public: + MD5(); + MD5(const void *input, size_t length); + MD5(const string &str); + MD5(ifstream &in); + void update(const void *input, size_t length); + void update(const string &str); + void update(ifstream &in); + const byte *digest(); + string toString(); + void reset(); + +private: + void update(const byte *input, size_t length); + void final(); + void transform(const byte block[64]); + void encode(const uint32 *input, byte *output, size_t length); + void decode(const byte *input, uint32 *output, size_t length); + string bytesToHexString(const byte *input, size_t length); + + /* class uncopyable */ + MD5(const MD5 &); + MD5 &operator=(const MD5 &); + +private: + uint32 _state[4]; /* state (ABCD) */ + uint32 _count[2]; /* number of bits, modulo 2^64 (low-order word first) */ + byte _buffer[64]; /* input buffer */ + byte _digest[16]; /* message digest */ + bool _finished; /* calculate finished ? */ + + static const byte PADDING[64]; /* padding for calculate */ + static const char HEX[16]; + enum + { + BUFFER_SIZE = 1024 + }; +}; + +#endif /*MD5_H*/ \ No newline at end of file diff --git a/src/sub/include/sub/sub_node.hpp b/src/sub/include/sub/sub_node.hpp new file mode 100644 index 0000000..fbf8002 --- /dev/null +++ b/src/sub/include/sub/sub_node.hpp @@ -0,0 +1,20 @@ + +#include +#ifndef __SUB_NODE_H__ +#define __SUB_NODE_H__ + +struct car_ctrl +{ + // 1. 使能模式 + int mode; // 0:手动驾驶, 3:远程驾驶 + + 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; + +#endif \ No newline at end of file diff --git a/src/remote/lib/libpaho-mqtt3c-static.a b/src/sub/lib/libpaho-mqtt3c-static.a similarity index 100% rename from src/remote/lib/libpaho-mqtt3c-static.a rename to src/sub/lib/libpaho-mqtt3c-static.a diff --git a/src/remote/lib/libpaho-mqtt3c.a b/src/sub/lib/libpaho-mqtt3c.a similarity index 100% rename from src/remote/lib/libpaho-mqtt3c.a rename to src/sub/lib/libpaho-mqtt3c.a diff --git a/src/remote/package.xml b/src/sub/package.xml similarity index 88% rename from src/remote/package.xml rename to src/sub/package.xml index 4008fb9..7e92f33 100644 --- a/src/remote/package.xml +++ b/src/sub/package.xml @@ -1,10 +1,10 @@ - remote + sub 0.0.0 TODO: Package description - zxwl + nvidia TODO: License declaration ament_cmake diff --git a/src/remote/src/jsoncpp.cpp b/src/sub/src/jsoncpp.cpp similarity index 100% rename from src/remote/src/jsoncpp.cpp rename to src/sub/src/jsoncpp.cpp diff --git a/src/sub/src/md5.cpp b/src/sub/src/md5.cpp new file mode 100644 index 0000000..eb7f064 --- /dev/null +++ b/src/sub/src/md5.cpp @@ -0,0 +1,371 @@ +#include "md5.h" +#include +#include +#include + +// 重命名自定义的 byte 类型 +typedef unsigned char uchar; + +using namespace std; + +/* Constants for MD5Transform routine. */ +#define S11 7 +#define S12 12 +#define S13 17 +#define S14 22 +#define S21 5 +#define S22 9 +#define S23 14 +#define S24 20 +#define S31 4 +#define S32 11 +#define S33 16 +#define S34 23 +#define S41 6 +#define S42 10 +#define S43 15 +#define S44 21 + +/* F, G, H and I are basic MD5 functions. + */ +#define F(x, y, z) (((x) & (y)) | ((~x) & (z))) +#define G(x, y, z) (((x) & (z)) | ((y) & (~z))) +#define H(x, y, z) ((x) ^ (y) ^ (z)) +#define I(x, y, z) ((y) ^ ((x) | (~z))) + +/* ROTATE_LEFT rotates x left n bits. + */ +#define ROTATE_LEFT(x, n) (((x) << (n)) | ((x) >> (32 - (n)))) + +/* FF, GG, HH, and II transformations for rounds 1, 2, 3, and 4. +Rotation is separate from addition to prevent recomputation. +*/ +#define FF(a, b, c, d, x, s, ac) \ + { \ + (a) += F((b), (c), (d)) + (x) + ac; \ + (a) = ROTATE_LEFT((a), (s)); \ + (a) += (b); \ + } +#define GG(a, b, c, d, x, s, ac) \ + { \ + (a) += G((b), (c), (d)) + (x) + ac; \ + (a) = ROTATE_LEFT((a), (s)); \ + (a) += (b); \ + } +#define HH(a, b, c, d, x, s, ac) \ + { \ + (a) += H((b), (c), (d)) + (x) + ac; \ + (a) = ROTATE_LEFT((a), (s)); \ + (a) += (b); \ + } +#define II(a, b, c, d, x, s, ac) \ + { \ + (a) += I((b), (c), (d)) + (x) + ac; \ + (a) = ROTATE_LEFT((a), (s)); \ + (a) += (b); \ + } + +const uchar MD5::PADDING[64] = {0x80}; +const char MD5::HEX[16] = { + '0', '1', '2', '3', + '4', '5', '6', '7', + '8', '9', 'a', 'b', + 'c', 'd', 'e', 'f'}; + +/* Default construct. */ +MD5::MD5() +{ + reset(); +} + +/* Construct a MD5 object with a input buffer. */ +MD5::MD5(const void *input, size_t length) +{ + reset(); + update(input, length); +} + +/* Construct a MD5 object with a string. */ +MD5::MD5(const string &str) +{ + reset(); + update(str); +} + +/* Construct a MD5 object with a file. */ +MD5::MD5(ifstream &in) +{ + reset(); + update(in); +} + +/* Return the message-digest */ +const uchar *MD5::digest() +{ + if (!_finished) + { + _finished = true; + final(); + } + return _digest; +} + +/* Reset the calculate state */ +void MD5::reset() +{ + _finished = false; + /* reset number of bits. */ + _count[0] = _count[1] = 0; + /* Load magic initialization constants. */ + _state[0] = 0x67452301; + _state[1] = 0xefcdab89; + _state[2] = 0x98badcfe; + _state[3] = 0x10325476; +} + +/* Updating the context with a input buffer. */ +void MD5::update(const void *input, size_t length) +{ + const uchar *uinput = static_cast(input); + update(uinput, length); +} + +/* Updating the context with a string. */ +void MD5::update(const string &str) +{ + const uchar *uinput = reinterpret_cast(str.c_str()); + update(uinput, str.length()); +} + +/* Updating the context with a file. */ +void MD5::update(ifstream &in) +{ + if (!in) + { + return; + } + + std::streamsize length; + char buffer[1024]; + while (!in.eof()) + { + in.read(buffer, 1024); + length = in.gcount(); + if (length > 0) + { + const uchar *uinput = reinterpret_cast(buffer); + update(uinput, length); + } + } + in.close(); +} + +/* MD5 block update operation. Continues an MD5 message-digest +operation, processing another message block, and updating the +context. +*/ +void MD5::update(const uchar *input, size_t length) +{ + uint32 i, index, partLen; + + _finished = false; + + /* Compute number of bytes mod 64 */ + index = (uint32)((_count[0] >> 3) & 0x3f); + + /* update number of bits */ + if ((_count[0] += ((uint32)length << 3)) < ((uint32)length << 3)) + { + ++_count[1]; + } + _count[1] += ((uint32)length >> 29); + + partLen = 64 - index; + + /* transform as many times as possible. */ + if (length >= partLen) + { + memcpy(&_buffer[index], input, partLen); + transform(_buffer); + + for (i = partLen; i + 63 < length; i += 64) + { + transform(&input[i]); + } + index = 0; + } + else + { + i = 0; + } + + /* Buffer remaining input */ + memcpy(&_buffer[index], &input[i], length - i); +} + +/* MD5 finalization. Ends an MD5 message-_digest operation, writing the +the message _digest and zeroizing the context. +*/ +void MD5::final() +{ + uchar bits[8]; + uint32 oldState[4]; + uint32 oldCount[2]; + uint32 index, padLen; + + /* Save current state and count. */ + memcpy(oldState, _state, 16); + memcpy(oldCount, _count, 8); + + /* Save number of bits */ + encode(_count, bits, 8); + + /* Pad out to 56 mod 64. */ + index = (uint32)((_count[0] >> 3) & 0x3f); + padLen = (index < 56) ? (56 - index) : (120 - index); + update(PADDING, padLen); + + /* Append length (before padding) */ + update(bits, 8); + + /* Store state in digest */ + encode(_state, _digest, 16); + + /* Restore current state and count. */ + memcpy(_state, oldState, 16); + memcpy(_count, oldCount, 8); +} + +/* MD5 basic transformation. Transforms _state based on block. */ +/* MD5 basic transformation. Transforms _state based on block. */ +void MD5::transform(const uchar block[64]) +{ + uint32 a = _state[0], b = _state[1], c = _state[2], d = _state[3], x[16]; + + decode(block, x, 64); + + /* Round 1 */ + FF(a, b, c, d, x[0], S11, 0xd76aa478); /* 1 */ + FF(d, a, b, c, x[1], S12, 0xe8c7b756); /* 2 */ + FF(c, d, a, b, x[2], S13, 0x242070db); /* 3 */ + FF(b, c, d, a, x[3], S14, 0xc1bdceee); /* 4 */ + FF(a, b, c, d, x[4], S11, 0xf57c0faf); /* 5 */ + FF(d, a, b, c, x[5], S12, 0x4787c62a); /* 6 */ + FF(c, d, a, b, x[6], S13, 0xa8304613); /* 7 */ + FF(b, c, d, a, x[7], S14, 0xfd469501); /* 8 */ + FF(a, b, c, d, x[8], S11, 0x698098d8); /* 9 */ + FF(d, a, b, c, x[9], S12, 0x8b44f7af); /* 10 */ + FF(c, d, a, b, x[10], S13, 0xffff5bb1); /* 11 */ + FF(b, c, d, a, x[11], S14, 0x895cd7be); /* 12 */ + FF(a, b, c, d, x[12], S11, 0x6b901122); /* 13 */ + FF(d, a, b, c, x[13], S12, 0xfd987193); /* 14 */ + FF(c, d, a, b, x[14], S13, 0xa679438e); /* 15 */ + FF(b, c, d, a, x[15], S14, 0x49b40821); /* 16 */ + + /* Round 2 */ + GG(a, b, c, d, x[1], S21, 0xf61e2562); /* 17 */ + GG(d, a, b, c, x[6], S22, 0xc040b340); /* 18 */ + GG(c, d, a, b, x[11], S23, 0x265e5a51); /* 19 */ + GG(b, c, d, a, x[0], S24, 0xe9b6c7aa); /* 20 */ + GG(a, b, c, d, x[5], S21, 0xd62f105d); /* 21 */ + GG(d, a, b, c, x[10], S22, 0x2441453); /* 22 */ + GG(c, d, a, b, x[15], S23, 0xd8a1e681); /* 23 */ + GG(b, c, d, a, x[4], S24, 0xe7d3fbc8); /* 24 */ + GG(a, b, c, d, x[9], S21, 0x21e1cde6); /* 25 */ + GG(d, a, b, c, x[14], S22, 0xc33707d6); /* 26 */ + GG(c, d, a, b, x[3], S23, 0xf4d50d87); /* 27 */ + GG(b, c, d, a, x[8], S24, 0x455a14ed); /* 28 */ + GG(a, b, c, d, x[13], S21, 0xa9e3e905); /* 29 */ + GG(d, a, b, c, x[2], S22, 0xfcefa3f8); /* 30 */ + GG(c, d, a, b, x[7], S23, 0x676f02d9); /* 31 */ + GG(b, c, d, a, x[12], S24, 0x8d2a4c8a); /* 32 */ + + /* Round 3 */ + HH(a, b, c, d, x[5], S31, 0xfffa3942); /* 33 */ + HH(d, a, b, c, x[8], S32, 0x8771f681); /* 34 */ + HH(c, d, a, b, x[11], S33, 0x6d9d6122); /* 35 */ + HH(b, c, d, a, x[14], S34, 0xfde5380c); /* 36 */ + HH(a, b, c, d, x[1], S31, 0xa4beea44); /* 37 */ + HH(d, a, b, c, x[4], S32, 0x4bdecfa9); /* 38 */ + HH(c, d, a, b, x[7], S33, 0xf6bb4b60); /* 39 */ + HH(b, c, d, a, x[10], S34, 0xbebfbc70); /* 40 */ + HH(a, b, c, d, x[13], S31, 0x289b7ec6); /* 41 */ + HH(d, a, b, c, x[0], S32, 0xeaa127fa); /* 42 */ + HH(c, d, a, b, x[3], S33, 0xd4ef3085); /* 43 */ + HH(b, c, d, a, x[6], S34, 0x4881d05); /* 44 */ + HH(a, b, c, d, x[9], S31, 0xd9d4d039); /* 45 */ + HH(d, a, b, c, x[12], S32, 0xe6db99e5); /* 46 */ + HH(c, d, a, b, x[15], S33, 0x1fa27cf8); /* 47 */ + HH(b, c, d, a, x[2], S34, 0xc4ac5665); /* 48 */ + + /* Round 4 */ + II(a, b, c, d, x[0], S41, 0xf4292244); /* 49 */ + II(d, a, b, c, x[7], S42, 0x432aff97); /* 50 */ + II(c, d, a, b, x[14], S43, 0xab9423a7); /* 51 */ + II(b, c, d, a, x[5], S44, 0xfc93a039); /* 52 */ + II(a, b, c, d, x[12], S41, 0x655b59c3); /* 53 */ + II(d, a, b, c, x[3], S42, 0x8f0ccc92); /* 54 */ + II(c, d, a, b, x[10], S43, 0xffeff47d); /* 55 */ + II(b, c, d, a, x[1], S44, 0x85845dd1); /* 56 */ + II(a, b, c, d, x[8], S41, 0x6fa87e4f); /* 57 */ + II(d, a, b, c, x[15], S42, 0xfe2ce6e0); /* 58 */ + II(c, d, a, b, x[6], S43, 0xa3014314); /* 59 */ + II(b, c, d, a, x[13], S44, 0x4e0811a1); /* 60 */ + II(a, b, c, d, x[4], S41, 0xf7537e82); /* 61 */ + II(d, a, b, c, x[11], S42, 0xbd3af235); /* 62 */ + II(c, d, a, b, x[2], S43, 0x2ad7d2bb); /* 63 */ + II(b, c, d, a, x[9], S44, 0xeb86d391); /* 64 */ + + _state[0] += a; + _state[1] += b; + _state[2] += c; + _state[3] += d; +} + +/* Encodes input (ulong) into output (byte). Assumes length is +a multiple of 4. +*/ +void MD5::encode(const uint32 *input, uchar *output, size_t length) +{ + for (size_t i = 0, j = 0; j < length; ++i, j += 4) + { + output[j] = (uchar)(input[i] & 0xff); + output[j + 1] = (uchar)((input[i] >> 8) & 0xff); + output[j + 2] = (uchar)((input[i] >> 16) & 0xff); + output[j + 3] = (uchar)((input[i] >> 24) & 0xff); + } +} + +/* Decodes input (byte) into output (ulong). Assumes length is +a multiple of 4. +*/ +void MD5::decode(const uchar *input, uint32 *output, size_t length) +{ + for (size_t i = 0, j = 0; j < length; ++i, j += 4) + { + output[i] = ((uint32)input[j]) | (((uint32)input[j + 1]) << 8) | + (((uint32)input[j + 2]) << 16) | (((uint32)input[j + 3]) << 24); + } +} + +/* Convert byte array to hex string. */ +string MD5::bytesToHexString(const uchar *input, size_t length) +{ + string str; + str.reserve(length << 1); + for (size_t i = 0; i < length; ++i) + { + int t = input[i]; + int a = t / 16; + int b = t % 16; + str.append(1, HEX[a]); + str.append(1, HEX[b]); + } + return str; +} + +/* Convert digest to string value */ +string MD5::toString() +{ + return bytesToHexString(digest(), 16); +} \ No newline at end of file