Merge branch 'master' of http://118.24.47.114/madesheng/self-driving-robot

src/decision/CMakeLists.txt

@@ -117,6 +117,7 @@ include_directories(
 # include
  ${catkin_INCLUDE_DIRS}
     /usr/local/include
+
 )
 
 ## Declare a C++ library
@@ -210,6 +211,8 @@ add_executable(decision
     ../common/gloghelper.cpp
     ../common/robotapp.h
     ../common/robotapp.cpp
+    ../common/sqlite3helper.h
+    ../common/sqlite3helper.cpp
     decision_app.cpp
     decision.h
     decision.cpp
@@ -222,4 +225,6 @@ target_link_libraries(decision
     gflags
     pcan
     dl
-    protobuf)
+    protobuf
+    sqlite3
+    )

src/decision/decision.cpp

@@ -1,5 +1,6 @@
 #include "decision.h"
 
+#include <string.h>
 
 
 namespace robot {
@@ -18,11 +19,387 @@ int Decision::Start(){
     LOG(INFO)<<"Decision start...";
     return ROBOT_SUCCESS;
 }
+
 void Decision::Stop(){
-    LOG(INFO)<<"Decision stop...";
+    LOG(INFO)<<Name()<<":Decision stop...";
     return ;
 }
 
+int Decision::InitDB(){
+    LOG(INFO)<<Name()<<":Decision database init...";
+
+    /***************************打开数据库 start ****************************/
+    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
+    if(sqlite==nullptr){
+        LOG(INFO)<<Name()<<":Decision opendb fail";
+        return 0;
+    }
+    /***************************打开数据库 end ******************************/
+
+    /****************检查,初始化表 action_directive start *******************/
+    int haveTable = robot::common::Sqlite3helper::Instance().isTableExits(sqlite,"action_directive");
+    if(haveTable ==-1){
+        LOG(INFO)<<Name()<<":Decision exits table ->action_directive fail";
+        return 0;
+    }else if(haveTable == 0){
+        int createResult = robot::common::Sqlite3helper::Instance().createTable(sqlite,
+                                       "CREATE TABLE action_directive (         \
+                                       rule_name        VARCHAR (50)  NOT NULL, \
+                                       directive_id     BIGINT        NOT NULL, \
+                                       directive_info   VARCHAR (255),          \
+                                       directive_result INT,                    \
+                                       PRIMARY KEY (                            \
+                                           rule_name,                           \
+                                           directive_id                         \
+                                       )                                        \
+                                       );"
+        );
+        if(createResult == 0){
+            LOG(INFO)<<Name()<<":Decision create table ->action_directive fail";
+            return 0;
+        }
+    }
+    /****************检查,初始化表 action_directive end **********************/
+
+    /******************检查,初始化表 location_map start **********************/
+    haveTable = robot::common::Sqlite3helper::Instance().isTableExits(sqlite,"location_map");
+    if(haveTable ==-1){
+        LOG(INFO)<<Name()<<":Decision exits table -> location_map fail";
+        return 0;
+    }else if(haveTable == 0){
+        int createResult = robot::common::Sqlite3helper::Instance().createTable(sqlite,
+                                       "CREATE TABLE location_map (             \
+                                       location_lon_1 FLOAT (53)    NOT NULL,   \
+                                       location_lat_1 FLOAT (53)    NOT NULL,   \
+                                       location_lon_2 FLOAT (53)    NOT NULL,   \
+                                       location_lat_2 FLOAT (53)    NOT NULL,   \
+                                       location_X_1   FLOAT (53)    NOT NULL,   \
+                                       location_Y_1   FLOAT (53)    NOT NULL,   \
+                                       map_id         VARCHAR (100) PRIMARY KEY \
+                                                                    NOT NULL,   \
+                                       location_X_2   FLOAT (53)    NOT NULL,   \
+                                       location_Y_2   FLOAT (53)    NOT NULL    \
+                                       );"
+        );
+        if(createResult == 0){
+            LOG(INFO)<<Name()<<":Decision create table -> location_map fail";
+            return 0;
+        }
+    }
+    /******************检查,初始化表 location_map end ************************/
+
+    /**************检查,初始化表 location_rule_info start ********************/
+    haveTable = robot::common::Sqlite3helper::Instance().isTableExits(sqlite,"location_rule_info");
+    if(haveTable ==-1){
+        LOG(INFO)<<Name()<<":Decision exits table -> location_rule_info fail";
+        return 0;
+    }else if(haveTable == 0){
+        int createResult = robot::common::Sqlite3helper::Instance().createTable(sqlite,
+                                       "CREATE TABLE location_rule_info (       \
+                                       rule_name       VARCHAR (50) NOT NULL,   \
+                                       location_id     BIGINT       NOT NULL,   \
+                                       location_lon    DECIMAL (38) NOT NULL,   \
+                                       location_lat    DECIMAL (38) NOT NULL,   \
+                                       location_height DECIMAL (38) NOT NULL,   \
+                                       location_speed  FLOAT (53)   NOT NULL,   \
+                                       PRIMARY KEY (                            \
+                                           rule_name,                           \
+                                           location_id                          \
+                                       )                                        \
+                                       );"
+        );
+        if(createResult == 0){
+            LOG(INFO)<<Name()<<":Decision create table -> location_map fail";
+            return 0;
+        }
+    }
+    /**************检查,初始化表 location_rule_info end **********************/
+    robot::common::Sqlite3helper::Instance().close(sqlite);
+    return 1;
+}
+
+int Decision::InsertActionDirective(const char *ruleName, int iDirectiveID, const char *directiveInfo, int iDirectiveResult){
+    LOG(INFO)<<Name()<<"insert into action_directive "<<
+               "rule_name="<<*ruleName<<",<<"
+               "directive_id="<<iDirectiveID<<",<<"
+               "directive_info="<<directiveInfo<<",<<"
+               "directive_result="<<iDirectiveResult;
+
+    /***************************打开数据库 start ****************************/
+    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
+    if(sqlite==nullptr){
+        LOG(INFO)<<Name()<<":Decision opendb fail";
+        return 0;
+    }
+    /***************************打开数据库 end ******************************/
+
+    /********************向表中插入一条记录 start *****************************/
+    std::string sRuleName = ruleName;
+    std::string sDirectiveID = std::to_string(iDirectiveID);
+    std::string sDirectiveInfo = directiveInfo;
+    std::string sDirectiveResult = std::to_string(iDirectiveResult);
+    std::string s_sql="INSERT INTO action_directive (   \
+            rule_name,                                  \
+            directive_id,                               \
+            directive_info,                             \
+            directive_result                            \
+        )                                               \
+        VALUES (\'"+sRuleName+"\',                      \
+                  "+sDirectiveID+",                     \
+                \'"+sDirectiveInfo+"\',                 \
+                  "+sDirectiveResult+");";
+    const char* sql=s_sql.c_str();
+    robot::common::Sqlite3helper::Instance().update(sqlite,sql);
+    /********************向表中插入一条记录 end *******************************/
+
+    robot::common::Sqlite3helper::Instance().close(sqlite);
+    return 1;
+}
+
+int Decision::InsertLocationMap(float fLocationLon1,
+                                float fLocationLat1,
+                                float fLocationLon2,
+                                float fLocationLat2,
+                                float fLocationX1,
+                                float fLocationY1,
+                                const char *mapId,
+                                float fLocationX2,
+                                float fLocationY2){
+
+    LOG(INFO)<<Name()<<"insert into location_map "<<
+               "location_lon_1="<<fLocationLon1<<","<<
+               "location_lat_1="<<fLocationLat1<<","<<
+               "location_lon_2="<<fLocationLon2<<","<<
+               "location_lat_2="<<fLocationLat2<<","<<
+               "location_location_X_1="<<fLocationX1<<","<<
+               "location_location_Y_2="<<fLocationY1<<","<<
+               "map_id="<<*mapId<<","<<","<<
+               "location_X_2="<<fLocationX2<<","<<
+               "location_Y_2="<<fLocationY2;
+    /***************************打开数据库 start ****************************/
+    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
+    if(sqlite==nullptr){
+        LOG(INFO)<<Name()<<"Decision opendb fail";
+        return 0;
+    }
+    /***************************打开数据库 end ******************************/
+
+    /********************向表中插入一条记录 start *****************************/
+    std::string sLocationLon1=std::to_string(fLocationLon1);
+    std::string sLocationLat1=std::to_string(fLocationLat1);
+    std::string sLocationLon2=std::to_string(fLocationLon2);
+    std::string sLocationLat2=std::to_string(fLocationLat2);
+    std::string sLocationX1=std::to_string(fLocationX1);
+    std::string sLocationY1=std::to_string(fLocationY1);
+    std::string sMapId=mapId;
+    std::string sLocationX2=std::to_string(fLocationX2);
+    std::string sLocationY2=std::to_string(fLocationY2);
+
+    std::string s_sql="INSERT INTO location_map (   \
+            location_lon_1,                         \
+            location_lat_1,                         \
+            location_lon_2,                         \
+            location_lat_2,                         \
+            location_X_1,                           \
+            location_Y_1,                           \
+            map_id,                                 \
+            location_X_2,                           \
+            location_Y_2                            \
+        )                                           \
+        VALUES (                                    \
+            "+sLocationLon1+",                      \
+            "+sLocationLat1+",                      \
+            "+sLocationLon2+",                      \
+            "+sLocationLat2+",                      \
+            "+sLocationX1+",                        \
+            "+sLocationY1+",                        \
+            \'"+sMapId+"\',                         \
+            "+sLocationX2+",                        \
+            "+sLocationY2+"                         \
+        );";
+    const char* sql=s_sql.c_str();
+    robot::common::Sqlite3helper::Instance().update(sqlite,sql);
+    /********************向表中插入一条记录 end *******************************/
+
+    robot::common::Sqlite3helper::Instance().close(sqlite);
+    return 1;
+
+}
+
+int Decision::InsertLocationRuleInfo(const char *ruleName,
+                                     int iLocationID,
+                                     double dLocationLon,
+                                     double dLocationLat,
+                                     double dLocationHeight,
+                                     float fLocationSpeed){
+    LOG(INFO)<<Name()<<"insert into location_rule_info "<<
+               "rule_name="<<*ruleName<<","<<
+               "location_id="<<iLocationID<<","<<
+               "location_lon="<<dLocationLon<<","<<
+               "location_lat="<<dLocationLat<<","<<
+               "location_height="<<dLocationHeight<<","<<
+               "location_speed="<<fLocationSpeed;
+
+    /***************************打开数据库 start ****************************/
+    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
+    if(sqlite==nullptr){
+        LOG(INFO)<<Name()<<"Decision opendb fail";
+        return 0;
+    }
+    /***************************打开数据库 end ******************************/
+
+    /********************向表中插入一条记录 start *****************************/
+    std::string sRuleName=ruleName;
+    std::string sLocationID=std::to_string(iLocationID);
+    std::string sLocationLon=std::to_string(dLocationLon);
+    std::string sLocationLat=std::to_string(dLocationLat);
+    std::string sLocationHeight=std::to_string(dLocationHeight);
+    std::string sLocationSpeed=std::to_string(fLocationSpeed);
+    std::string s_sql="INSERT INTO location_rule_info ( \
+            rule_name,                                  \
+            location_id,                                \
+            location_lon,                               \
+            location_lat,                               \
+            location_height,                            \
+            location_speed                              \
+        )                                               \
+        VALUES (                                        \
+            \'"+sRuleName+"\',                          \
+            "+sLocationID+",                            \
+            "+sLocationLon+",                           \
+            "+sLocationLat+",                           \
+            "+sLocationHeight+",                        \
+            "+sLocationSpeed+"                          \
+        );";
+    const char* sql=s_sql.c_str();
+    robot::common::Sqlite3helper::Instance().update(sqlite,sql);
+    /********************向表中插入一条记录 end *******************************/
+
+    robot::common::Sqlite3helper::Instance().close(sqlite);
+    return 1;
+}
+
+int Decision::DeleteActionDirective(const char *ruleName){
+    LOG(INFO)<<Name()<<"delete action_directive where rule_name="<<*ruleName;
+
+    /***************************打开数据库 start ****************************/
+    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
+    if(sqlite==nullptr){
+        LOG(INFO)<<Name()<<"Decision opendb fail";
+        return 0;
+    }
+    /***************************打开数据库 end ******************************/
+
+    std::string sRuleName= ruleName;
+    std::string s_sql="DELETE FROM action_directive WHERE rule_name=" + sRuleName;
+    const char* sql=s_sql.c_str();
+    int deleteResult = robot::common::Sqlite3helper::Instance().update(sqlite,sql);
+    robot::common::Sqlite3helper::Instance().close(sqlite);
+    if(deleteResult == 0){
+        return 0;
+    }else{
+        return 1;
+    }
+
+}
+
+int Decision::DeleteActionDirective(int iDirectiveID){
+    LOG(INFO)<<Name()<<"delete action_directive where directive_id="<<iDirectiveID;
+
+    /***************************打开数据库 start ****************************/
+    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
+    if(sqlite==nullptr){
+        LOG(INFO)<<Name()<<"Decision opendb fail";
+        return 0;
+    }
+    /***************************打开数据库 end ******************************/
+
+    std::string sDirectiveID=std::to_string(iDirectiveID);
+    std::string s_sql="DELETE FROM action_directive WHERE directive_id=" + sDirectiveID+";";
+    const char* sql=s_sql.c_str();
+    int deleteResult = robot::common::Sqlite3helper::Instance().update(sqlite,sql);
+    robot::common::Sqlite3helper::Instance().close(sqlite);
+    if(deleteResult == 0){
+        return 0;
+    }else{
+        return 1;
+    }
+
+}
+
+int Decision::DeleteLocationMap(const char *mapID){
+    LOG(INFO)<<Name()<<"delete location_map where map_id="<<*mapID;
+
+    /***************************打开数据库 start ****************************/
+    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
+    if(sqlite==nullptr){
+        LOG(INFO)<<Name()<<"Decision opendb fail";
+        return 0;
+    }
+    /***************************打开数据库 end ******************************/
+
+    std::string sMapID=mapID;
+    std::string s_sql="DELETE FROM location_map WHERE map_id=" + sMapID;
+    const char* sql=s_sql.c_str();
+    int deleteResult = robot::common::Sqlite3helper::Instance().update(sqlite,sql);
+    robot::common::Sqlite3helper::Instance().close(sqlite);
+    if(deleteResult == 0){
+        return 0;
+    }else{
+        return 1;
+    }
+
+}
+
+int Decision::DeleteLocationRuleInfo(const char *ruleName){
+    LOG(INFO)<<Name()<<"delete location_rule_info where rule_name="<<*ruleName;
+
+    /***************************打开数据库 start ****************************/
+    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
+    if(sqlite==nullptr){
+        LOG(INFO)<<Name()<<"Decision opendb fail";
+        return 0;
+    }
+    /***************************打开数据库 end ******************************/
+
+    std::string sRuleName=ruleName;
+    std::string s_sql="DELETE FROM location_rule_info WHERE rule_name=" + sRuleName;
+    const char* sql=s_sql.c_str();
+    int deleteResult = robot::common::Sqlite3helper::Instance().update(sqlite,sql);
+    robot::common::Sqlite3helper::Instance().close(sqlite);
+    if(deleteResult == 0){
+        return 0;
+    }else{
+        return 1;
+    }
+
+}
+
+int Decision::DeleteLocationRuleInfo(int iLocationID){
+    LOG(INFO)<<Name()<<"delete location_rule_info where location_id="<<iLocationID;
+
+    /***************************打开数据库 start ****************************/
+    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
+    if(sqlite==nullptr){
+        LOG(INFO)<<Name()<<"Decision opendb fail";
+        return 0;
+    }
+    /***************************打开数据库 end ******************************/
+
+    std::string sLocationID=std::to_string(iLocationID);
+    std::string s_sql="DELETE FROM location_rule_info WHERE location_id=" + sLocationID;
+    const char* sql=s_sql.c_str();
+    int deleteResult = robot::common::Sqlite3helper::Instance().update(sqlite,sql);
+    robot::common::Sqlite3helper::Instance().close(sqlite);
+    if(deleteResult == 0){
+        return 0;
+    }else{
+        return 1;
+    }
+
+}
+
+
 }
 }
 

src/decision/decision.h

@@ -11,7 +11,7 @@
 
 #include "../common/robotapp.h"
 #include "ros/ros.h"
-
+#include "../common/sqlite3helper.h"
 
 namespace robot {
 namespace decision {
@@ -30,7 +30,9 @@ namespace decision {
 class Decision: public robot::common::RobotApp{
 
 public:
-    Decision(){}
+    Decision(){
+
+    }
     /**
     * decision节点名称
     */
@@ -56,6 +58,84 @@ public:
     * @return start status
     */
     void Stop() override;
+
+    /**
+    * @brief decision模块的数据库初始化，如果相关表不存在，则建立相关的表
+    * @brief 相关的表:action_directive,location_map,location_rule_info
+    * @return 0失败
+    */
+    int InitDB();
+
+    /**
+    * @brief 向action_firective表中插入一条
+    * @return 0失败
+    */
+    int InsertActionDirective(const char* ruleName,
+                              int iDirectiveID,
+                              const char* directiveInfo,
+                              int iDirectiveResult);
+
+    /**
+    * @brief 向location_map表中插入一条
+    * @return 0失败
+    */
+    int InsertLocationMap(float fLocationLon1,
+                          float fLocationLat1,
+                          float fLocationLon2,
+                          float fLocationLat2,
+                          float fLocationX1,
+                          float fLocationY1,
+                          const char * mapId ,
+                          float fLocationX2,
+                          float fLocationY2);
+
+    /**
+    * @brief 向location_rule_info表中插入一条
+    * @return 0失败
+    */
+    int InsertLocationRuleInfo(const char * ruleName,
+                               int iLocationID,
+                               double dLocationLon,
+                               double dLocationLat,
+                               double dLocationHeight,
+                               float fLocationSpeed
+                               );
+
+    /**
+    * @brief 在action_firective表中,
+    * @brief 删除rule_name的记录
+    * @return 0失败
+    */
+    int DeleteActionDirective(const char* ruleName);
+
+    /**
+    * @brief 在action_firective表中,
+    * @brief 删除directive_id的记录
+    * @return 0失败
+    */
+    int DeleteActionDirective(int iDirectiveID);
+
+    /**
+    * @brief 在location_map表中,
+    * @brief 删除map_id的记录
+    * @return 0失败
+    */
+    int DeleteLocationMap(const char* mapID);
+
+    /**
+    * @brief 在location_rule_info表中,
+    * @brief 删除rule_name的记录
+    * @return 0失败
+    */
+    int DeleteLocationRuleInfo(const char* RuleName);
+
+    /**
+    * @brief 在location_rule_info表中,
+    * @brief 删除rule_name的记录
+    * @return 0失败
+    */
+    int DeleteLocationRuleInfo(int iLocationID);
+
 };