remote -> sub

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
 {

src/remote/include/remote/remote_node.hpp

@@ -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

src/remote/src/remote_node.cpp

@@ -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 <fstream>
-#include <iostream>
-#include <string>
-#include <chrono>
-#include <ctime>
-#include <unistd.h>
-#include <string.h>
-#include <time.h>
-#include <sstream>
-#include <iomanip>
-#include <pthread.h>
-
-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<sweeperMsg::McCtrl>("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<float>(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<float>((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<sweeperMsg::McCtrl>::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>("remote_node");
-    /* 运行节点，并检测退出信号*/
-    rclcpp::spin(node);
-    rclcpp::shutdown();
-
-    return 0;
-}

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)  
   target_link_libraries(
-    remote_node
+    sub_node  
     ${PROJECT_SOURCE_DIR}/lib/libpaho-mqtt3c-static.a
   )
 else()  
   target_link_libraries(
-    remote_node
+    sub_node  
     ${PROJECT_SOURCE_DIR}/lib/libpaho-mqtt3c.a
   )  
 endif() 
 
 install(TARGETS
-  remote_node
+  sub_node
   DESTINATION lib/${PROJECT_NAME}
 )
 
@@ -64,5 +68,4 @@ if(BUILD_TESTING)
   #set(ament_cmake_cpplint_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
 endif() 
-
 ament_package()

src/sub/include/sub/md5.h

@@ -0,0 +1,56 @@
+#ifndef MD5_H
+#define MD5_H
+
+#include <string>
+#include <fstream>
+
+/* 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*/

src/sub/include/sub/sub_node.hpp

@@ -0,0 +1,20 @@
+
+#include <string>
+#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

src/sub/package.xml

@@ -1,10 +1,10 @@
 <?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>remote</name>
+  <name>sub</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
-  <maintainer email="zxwl@todo.todo">zxwl</maintainer>
+  <maintainer email="nvidia@todo.todo">nvidia</maintainer>
   <license>TODO: License declaration</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>

src/sub/src/md5.cpp

@@ -0,0 +1,371 @@
+#include "md5.h"
+#include <string.h>
+#include <string>
+#include <fstream>
+
+// 重命名自定义的 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<const uchar *>(input);
+    update(uinput, length);
+}
+
+/* Updating the context with a string. */
+void MD5::update(const string &str)
+{
+    const uchar *uinput = reinterpret_cast<const uchar *>(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<const uchar *>(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);
+}