src/canbus/canbus.cpp

@@ -289,7 +289,6 @@ void Canbus::publish_data(TPCANMsg *pMsgBuff)
     char strData[16]={0};
     MessageCoder::ConvertUnCharToStr(strData,pMsgBuff->DATA,8);
     LOG(INFO)<<Name()<<" Publish CanData :" << "ID:" << strId << ",Data:"<<strData;
-    LOG(INFO)<<Name()<<" Msg CanData :" << "ID:" << rosMsg.id << ",Data:"<<rosMsg.data;
 }
 
 int Canbus::write_data(TPCANMsg *pMsgBuff)

src/common/message.h

@@ -44,6 +44,8 @@ const std::string ShiftActCtrService = "shiftactctrservice";
 const std::string SteerActCtrService = "streeractstrservice";
 
 const std::string PedalActCtrService = "pedalactctrservice";
+
+const std::string CommandModeCmdService = "commandmodecmdservice";
 // -----------------services ----------------------------
 
 

src/common/mpc.cpp

@@ -17,7 +17,7 @@ double dt = 0.05; // 50ms
 const double Lf = 2.67;
 
 // 参考速度，为了避免车辆在行驶中停止
-double ref_v = 65;
+//double ref_v = 65;
 
 // solver使用的是一个向量存储所有的状态值，所以需要确定每种状态在向量中的开始位置
 size_t x_start = 0;
@@ -34,8 +34,10 @@ public:
 
     // 参考路径方程的系数
     Eigen::VectorXd coeffs;
-    FG_eval(Eigen::VectorXd coeffs) {
+    double ref_v = 0;
+    FG_eval(Eigen::VectorXd coeffs, double speed) {
         this->coeffs = coeffs;
+        ref_v = speed;
     }
 
     typedef CPPAD_TESTVECTOR(AD<double>)ADvector;
@@ -191,7 +193,7 @@ vector<vector<double>> MPC::Solve(Eigen::VectorXd x0, Eigen::VectorXd coeffs) {
     constraints_upperbound[epsi_start] = epsi;
 
     // Object that computes objective and constraints
-    FG_eval fg_eval(coeffs);
+    FG_eval fg_eval(coeffs, ref_speed);
 
     // options
     std::string options;

src/common/mpc.h

@@ -27,6 +27,7 @@ class MPC {
   double delta_prev {0};
   double a_prev {0.1};
   int latency = 2;
+  double ref_speed = 0;
 
   // Solve the model given an initial state and polynomial coefficients.
   // Return the first actuatotions.

src/decision/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 2.8.3)
-project(desision)
+project(decision)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++11)
@@ -8,7 +8,7 @@ add_compile_options(-std=c++11)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
-    roscpp message_generation std_msgs rospy
+    roscpp message_generation std_msgs rospy canbus
 )
 
 ## System dependencies are found with CMake's conventions
@@ -52,11 +52,10 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+ add_service_files(
+   FILES
+   CommandModeCmd.srv
+ )
 
 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -66,10 +65,10 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
+ generate_messages(
+   DEPENDENCIES
+   std_msgs  # Or other packages containing msgs
+ )
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -103,7 +102,8 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES desision
-#  CATKIN_DEPENDS other_catkin_pkg
+  CATKIN_DEPENDS #other_catkin_pkg
+  message_runtime
 #  DEPENDS system_lib
 )
 
@@ -207,25 +207,28 @@ include_directories(
 
 add_executable(decision
     ../common/time.h
+    ../common/message.h
     ../common/gloghelper.h
     ../common/gloghelper.cpp
     ../common/robotapp.h
     ../common/robotapp.cpp
     ../common/sqlite3helper.h
     ../common/sqlite3helper.cpp
+    ../common/mpc.h
+    ../common/mpc.cpp
     decision_app.cpp
     decision.h
     decision.cpp
     decisiondatabase.h
     decisiondatabase.cpp
  )
-
+add_dependencies(decision ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 target_link_libraries(decision
     ${catkin_LIBRARIES}
     glog
     gflags
-    pcan
+    ipopt
     dl
     protobuf
     sqlite3

src/decision/decision.cpp

@@ -1,10 +1,15 @@
 #include "decision.h"
-
+#include "../common/message.h"
+#include <strstream>
 #include <string.h>
+#include <canbus/PedalActCtrReq.h>
+#include <canbus/SftActCtrReq.h>
+#include <canbus/StrActCtrReq.h>
+#include <stdlib.h>
 
 
 namespace robot {
-namespace decision {
+namespace decisionSpace {
 
 
 
@@ -13,28 +18,28 @@ std::string Decision::Name() const { return "decision"; }
 int Decision::Init(){
 
     LOG(INFO)<<Name()<<":Decision init...";
+    node_handle_.reset(new ros::NodeHandle());
     Database::InitDB();
-    Database::InsertActionDirective("1",1,"1",1);
-    Database::InsertLocationMap(0.1,0.1,0.1,0.1,0.1,0.1,"1",0.1,0.1);
-    Database::InsertLocationRuleInfo("1",1,0.1,0.1,0.1,0.1);
-    Database::UpdateActionDirective("1",1,2);
-    Database::UpdateLocationMap(0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2,"1");
-//    Database::DeleteActionDirective("1");
-    Database::DeleteActionDirective(1);
-    Database::DeleteLocationMap("1");
-//    Database::DeleteLocationRuleInfo("1");
-    Database::DeleteLocationRuleInfo(1);
-    return 0;
-
+    iCommandCmd = 0;// set undefied mode
+    realCarSpeed = localizationNumbers = RulesNumbers =0;
+    PredictionPoints = 20;
     LOG(INFO)<<Name()<<"Decision init...";
     return ROBOT_SUCCESS;
 
 }
 
 int Decision::Start(){
-    LOG(INFO)<<"Decision start...";
-//    ros::Subscriber sub = node_handle_->subscribe("canDataAnalysis", 20000, RTKLocationCallback);
+    LOG(INFO)<<Name()<<"Decision start...";
+    ros::ServiceServer CommandService = node_handle_->advertiseService(robot::common::CommandModeCmdService,
+                                                                &Decision::CommandRequest, this);
+    ros::Subscriber GeometrySub = node_handle_->subscribe(robot::common::Geometry, 20000,
+                                                          &Decision::RTKLocationCallback, this);
+
+    ros::Subscriber OBDSub = node_handle_->subscribe(robot::common::ObdData, 20000, &Decision::OBDCallback, this);
+
+//    ros::Subscriber RCUModeSub = node_handle_->subscribe(robot::common::AesData, &Decision::RCUModeCallback, this);
     ros::spin();
+     LOG(INFO)<<Name()<<"Decision end.";
     return ROBOT_SUCCESS;
 }
 
@@ -44,6 +49,231 @@ void Decision::Stop(){
 }
 
 
+bool Decision::CommandRequest(decision::CommandModeCmd::Request &req,
+                      decision::CommandModeCmd::Response &res)
+{
+
+    if (req.Cmd == CMD_AUTODRIE_START)
+    {
+        double breakCtr = -5;
+        ros::Rate loop_rate(1);
+        while(ros::ok())
+        {
+            if (SpeedControl(breakCtr) == 1)
+            {
+
+                if (BreakCtrControl() == 1)
+                    break;
+            }
+            ros::spinOnce();
+            loop_rate.sleep();
+        }
+
+    }
+    res.CmdResult = iCommandCmd = req.Cmd;
+    return true;
+}
+
+
+ void Decision::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
+ {
+
+     switch (iCommandCmd) {
+     case CMD_PLAN_START:
+         ExcutePlanRoute(msg);
+         break;
+     case CMD_AUTODRIE_START:
+         ExcuteSelfDriving(msg);
+         break;
+     case CMD_PLAN_END:
+         break;
+     case CMD_AUTODRIE_END:
+         break;
+     default:
+         break;
+     }
+ }
+
+
+void Decision::OBDCallback(const canbus::obdMsg::ConstPtr& msg)
+{
+    realCarSpeed = msg->VehSpd;
+}
+
+//void Decision::RCUModeCallback(const perception::aesMsg::ConstPtr& msg)
+//{
+
+//}
+
+void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
+{
+    try
+    {
+        localizationNumbers ++;
+        std::string PlanRouteName = "siasun";
+        if ((localizationNumbers % PredictionPoints)  == 0)
+        {
+            lastCarSpeed = realCarSpeed;
+            RulesNumbers++;
+            std::stringstream strRuleContent;
+            if (RulesNumbers == 1)
+                strRuleContent<<"动作"<<RulesNumbers<<":起步请加速到"<< realCarSpeed<<"KPH";
+            else{
+                if (realCarSpeed > lastCarSpeed)
+                {
+                    strRuleContent<<"动作"<<RulesNumbers<<":加速到"<< realCarSpeed<<"KPH";
+                }
+                else if (realCarSpeed < lastCarSpeed)
+                {
+                    strRuleContent<<"动作"<<RulesNumbers<<":制动至"<< realCarSpeed<<"KPH";
+                }
+                else if (realCarSpeed == lastCarSpeed)
+                {
+                    strRuleContent<<"动作"<<RulesNumbers<<":保持"<< realCarSpeed<<"KPH";
+                }
+           }
+           int iRet = Database::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), RulesNumbers,
+                            strRuleContent.str().c_str(), 0, localizationNumbers, msg->Longitude, msg->Latitude,
+                            msg->Height, realCarSpeed);
+           if (iRet < 0) {
+                localizationNumbers--;
+                RulesNumbers--;
+           }
+        }
+        else
+        {
+            Database::InsertLocationRuleInfo(PlanRouteName.c_str(),localizationNumbers, msg->Longitude,
+                                             msg->Latitude, msg->Height, realCarSpeed);
+        }
+    }
+    catch (ros::Exception ex)
+    {
+        LOG(ERROR)<<Name()<<":ExcutePlanRoute error."<< ex.what();
+    }
+}
+
+
+void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
+{
+   // 判断当前RCU是否为自动驾驶模式
+}
+
+void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &ptsy,
+                              double &px, double &py,  double &psi, double &v)
+{
+     // 转换坐标到当前车辆坐标
+     Eigen::MatrixXd waypoints = transformGlobal2Vehicle(px, py, psi, ptsx, ptsy);
+     Eigen::VectorXd Ptsx = waypoints.row(0);
+     Eigen::VectorXd Ptsy = waypoints.row(1);
+     // 获取状态值
+     auto coeffs = polyfit(Ptsx, Ptsy, 3);
+     double cte = evaluateCte(coeffs);
+     double epsi = evaluateEpsi(coeffs);
+     Eigen::VectorXd state(6);
+     state << 0, 0, 0, v, cte, epsi;
+     // 计算方向和油门，范围都在[-1, 1]之间
+     // 这个地方有延迟，取第三个
+     vector<vector<double>> result = mpc.Solve(state, coeffs);
+     double steer_value = result[2][mpc.latency];
+     double throttle_value = result[3][mpc.latency];
+     mpc.delta_prev = steer_value;
+     mpc.a_prev = throttle_value;
+     SpeedControl(steer_value);
+     StreeControl(throttle_value);
+}
+
+unsigned int Decision::SpeedControl(double &speed)
+{
+     ros::ServiceClient clientPedalActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::PedalActCtrService);
+     canbus::PedalActCtrReq srv;
+     if (mpc.a_prev >0){
+         srv.request.uiAccPsngCtrReq =(unsigned int)(speed *10);
+         srv.request.uiBrkPsngCtrReq =0;
+     }
+     else{
+         srv.request.uiAccPsngCtrReq = 0;
+         srv.request.uiBrkPsngCtrReq =(unsigned int)(speed *10);
+     }
+     srv.request.iAccSpdCtrReq = 20;
+     srv.request.iBrkSpdCtrReq = 50;
+     srv.request.uiBrkCtrMdReq = 1;
+     srv.request.uiBrkFocCtrReq = 350;
+     if (clientPedalActCtrReq.call(srv))
+     {
+         LOG(INFO)<<srv.response.uiReturn;
+     }
+    return srv.response.uiReturn;
+}
+unsigned int Decision::StreeControl(double &streer)
+{
+     ros::ServiceClient clientStrActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::SteerActCtrService);
+     canbus::StrActCtrReq srv;
+     srv.request.iStrAngCtrReq =(unsigned int)(streer *100);
+     srv.request.uiStrCtrMdReq =1;
+     srv.request.iStrSpdCtrReq = 200;
+     srv.request.iStrTrqCtrReq = 50;
+     if (clientStrActCtrReq.call(srv))
+     {
+         LOG(INFO)<<srv.response.uiReturn;
+     }
+     return srv.response.uiReturn;
+}
+
+unsigned int Decision::BreakCtrControl()
+{
+     ros::ServiceClient clientSftActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::ShiftActCtrService);
+     canbus::SftActCtrReq srv;
+     srv.request.uiSftPsngCtrReq = SFT_PSNG_LRN_REQ_FORWARD_1;
+     srv.request.uiSftMdReq = SFT_MD_REQ_2_SECOND;
+     srv.request.uiClchPsngCtrReq = 1;
+     srv.request.iClchSpdCtrReq = 0;
+     if (clientSftActCtrReq.call(srv))
+     {
+         LOG(INFO)<<srv.response.uiReturn;
+     }
+     return srv.response.uiReturn;
+}
+
+ // 根据输入值x,y解方程
+ Eigen::VectorXd Decision::polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order) {
+     assert(xvals.size() == yvals.size());
+     assert(order >= 1 && order <= xvals.size() - 1);
+     Eigen::MatrixXd A(xvals.size(), order + 1);
+     for (int i = 0; i < xvals.size(); i++) {
+         A(i, 0) = 1.0;
+     }
+     for (int j = 0; j < xvals.size(); j++) {
+         for (int i = 0; i < order; i++) {
+             A(j, i + 1) = A(j, i) * xvals(j);
+         }
+     }
+     auto Q = A.householderQr();
+     auto result = Q.solve(yvals);
+     return result;
+ }
+
+// 计算CTE误差
+ double Decision::evaluateCte(Eigen::VectorXd coeffs) {
+   return polyeval(coeffs, 0);
+ }
+
+ // 计算Epsi误差
+ double Decision::evaluateEpsi(Eigen::VectorXd coeffs) {
+   return -atan(coeffs[1]);
+ }
+
+ // 把全局坐标转换为汽车坐标，以当前汽车的位置作为远点
+ Eigen::MatrixXd Decision::transformGlobal2Vehicle(double x, double y, double psi,
+        const vector<double> & ptsx, const vector<double> & ptsy) {
+    assert(ptsx.size() == ptsy.size());
+    unsigned len = ptsx.size();
+    auto waypoints = Eigen::MatrixXd(2, len);
+    for (auto i = 0; i < len; ++i) {
+        waypoints(0, i) = cos(psi) * (ptsx[i] - x) + sin(psi) * (ptsy[i] - y);
+        waypoints(1, i) = -sin(psi) * (ptsx[i] - x) + cos(psi) * (ptsy[i] - y);
+    }
+    return waypoints;
+}
 
 }
 }

src/decision/decision.h

@@ -12,14 +12,42 @@
 #include "../common/robotapp.h"
 #include "ros/ros.h"
 #include "../common/sqlite3helper.h"
+#include "../common/mpc.h"
 #include "decisiondatabase.h"
+#include <decision/CommandModeCmd.h>
+#include <localization/localMsg.h>
+#include <canbus/obdMsg.h>
+#include <perception/aesMsg.h>
+#include <vector>
+#include <math.h>
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/QR>
 
 namespace robot {
-namespace decision {
+namespace decisionSpace {
 
 #define ROBOT_SUCCESS     0
 #define ROBOT_FAILTURE   -1
 
+#define CMD_UNDEFIED         0
+#define CMD_PLAN_START       1
+#define CMD_AUTODRIE_START   2
+#define CMD_PLAN_END         3
+#define CMD_AUTODRIE_END     4
+
+#define	SFT_PSNG_LRN_REQ_PARK			1	//挡位位置学习请求-停车档（自动档）
+#define	SFT_PSNG_LRN_REQ_REVERSE		2	//挡位位置学习请求-倒档
+#define	SFT_PSNG_LRN_REQ_NEUTRAL		3	//挡位位置学习请求-空档
+#define	SFT_PSNG_LRN_REQ_FORWARD_1		4	//挡位位置学习请求-前进档1(手动档)
+#define	SFT_MD_REQ_3_SECOND		0			//换挡模式-平滑型
+#define	SFT_MD_REQ_2_SECOND		1			//换挡模式-快速型
+#define	SFT_MD_REQ_1_SECOND		2			//换挡模式-激进型
+
+// 角度和弧度之间的转换
+constexpr double pi() {
+    return M_PI;
+}
+
 /**
 * @class Decision
 *
@@ -61,7 +89,71 @@ public:
     void Stop() override;
 
 private:
-    //void RTKLocationCallback(const loca::canMsg::ConstPtr& msg);
+    void RTKLocationCallback(const localization::localMsg::ConstPtr& msg);
+
+    void OBDCallback(const canbus::obdMsg::ConstPtr& msg);
+
+    //void RCUModeCallback(const perception::aesMsg::ConstPtr& msg);
+
+    bool CommandRequest(decision::CommandModeCmd::Request &req,
+                        decision::CommandModeCmd::Response &res);
+
+    void ExcutePlanRoute(const localization::localMsg::ConstPtr& msg);
+
+    void ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg);
+
+    void MPC_Controller(std::vector<double> &ptsx, std::vector<double> &ptsy,
+                        double &px, double &py,  double &psi, double &v);
+
+
+    inline double deg2rad(double x) {
+        return x * pi() / 180;
+    }
+    inline double rad2deg(double x) {
+        return x * 180 / pi();
+    }
+
+    // 根据系数和x值获取y值
+    inline double polyeval(Eigen::VectorXd coeffs, double x) {
+        double result = 0.0;
+        for (int i = 0; i < coeffs.size(); i++) {
+            result += coeffs[i] * pow(x, i);
+        }
+        return result;
+    }
+
+    // 根据输入值x,y解方程
+    Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order);
+
+    // 计算CTE误差
+    double evaluateCte(Eigen::VectorXd coeffs);
+    // 计算Epsi误差
+    double evaluateEpsi(Eigen::VectorXd coeffs);
+
+    // 把全局坐标转换为汽车坐标，以当前汽车的位置作为远点
+    Eigen::MatrixXd transformGlobal2Vehicle(double x, double y, double psi,
+           const vector<double> & ptsx, const vector<double> & ptsy);
+    unsigned int SpeedControl(double &speed);
+    unsigned int StreeControl(double &streer);
+
+    unsigned int BreakCtrControl();
+
+
+
+private:
+    int32_t iCommandCmd; // excute mode 0: undefined 1: plan start 2: auto-drive start  3: plan stop 4: auto-drive stop
+
+    float_t realCarSpeed;
+
+    float_t lastCarSpeed; //plan
+
+    int64_t localizationNumbers;
+
+    int64_t RulesNumbers;
+
+    int32_t PredictionPoints;
+
+    robot::common::MPC mpc;
 
 };
 

src/decision/decision_app.cpp

@@ -1,5 +1,5 @@
 #include "../common/robotapp.h"
 #include "decision.h"
 
-ROBOT_MAIN(robot::decision::Decision);
+ROBOT_MAIN(robot::decisionSpace::Decision);
 

src/decision/decisiondatabase.cpp

@@ -1,7 +1,7 @@
 #include "decisiondatabase.h"
 #include <string>
 namespace robot {
-namespace decision {
+namespace decisionSpace {
 
 
 std::string Database::Name() { return "Database"; }
@@ -198,7 +198,7 @@ int Database::InsertLocationRuleInfo(const char *ruleName,
                                      int iLocationID,
                                      double dLocationLon,
                                      double dLocationLat,
-                                     double dLocationHeight,
+                                     float dLocationHeight,
                                      float fLocationSpeed){
 
     /***************************打开数据库 start ****************************/
@@ -241,6 +241,86 @@ int Database::InsertLocationRuleInfo(const char *ruleName,
     return 1;
 }
 
+int Database::InsertLocationRuleInfoAndActionDirective(const char* ruleName,
+                                                    int iDirectiveID,
+                                                    const char* directiveInfo,
+                                                    int iDirectiveResult,
+                                                    int iLocationID,
+                                                    double dLocationLon,
+                                                    double dLocationLat,
+                                                    float dLocationHeight,
+                                                    float fLocationSpeed)
+{
+    int iRet = 0;
+    /***************************打开数据库 start ****************************/
+    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
+    if(sqlite==nullptr){
+        LOG(INFO)<<Name()<<"Decision opendb fail";
+        return 0;
+    }
+    /***************************打开数据库 end ******************************/
+    robot::common::Sqlite3helper::Instance().transaction(sqlite);
+    try
+    {
+
+
+        /********************向表中插入一条记录 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_sqlsqlLocation="INSERT INTO location_rule_info ( \
+                rule_name,                                  \
+                location_id,                                \
+                location_lon,                               \
+                location_lat,                               \
+                location_height,                            \
+                location_speed                              \
+            )                                               \
+            VALUES (                                        \
+                \'"+sRuleName+"\',                          \
+                "+sLocationID+",                            \
+                "+sLocationLon+",                           \
+                "+sLocationLat+",                           \
+                "+sLocationHeight+",                        \
+                "+sLocationSpeed+"                          \
+            );";
+        LOG(INFO)<<s_sqlsqlLocation;
+        const char* sqlLocation=s_sqlsqlLocation.c_str();
+        robot::common::Sqlite3helper::Instance().update(sqlite, sqlLocation);
+
+        //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+");";
+        LOG(INFO)<<s_sql;
+        const char* sql=s_sql.c_str();
+        robot::common::Sqlite3helper::Instance().update(sqlite,sql);
+        /********************向表中插入一条记录 end *******************************/
+         robot::common::Sqlite3helper::Instance().commitTransaction(sqlite);
+         iRet =1;
+    }
+    catch (ros::Exception ex)
+    {
+        robot::common::Sqlite3helper::Instance().rollbackTransaction(sqlite);
+        iRet = -1;
+    }
+    robot::common::Sqlite3helper::Instance().close(sqlite);
+    return iRet;
+}
+
 int Database::DeleteActionDirective(const char *ruleName){
 
     /***************************打开数据库 start ****************************/

src/decision/decisiondatabase.h

@@ -6,7 +6,10 @@
 #include "../common/sqlite3helper.h"
 
 namespace robot {
-namespace decision {
+namespace decisionSpace {
+
+#define DATABASE_SUCCESS     0
+#define DATABASE_FAILTURE   -1
 
 class Database{
 
@@ -22,7 +25,7 @@ public :
     static int InitDB();
 
     /**
-    * @brief 向action_firective表中插入一条
+    * @brief 向action_Directive表中插入一条
     * @return 0失败
     */
     static int InsertActionDirective(const char* ruleName,
@@ -46,16 +49,32 @@ public :
 
     /**
     * @brief 向location_rule_info表中插入一条
+    *
     * @return 0失败
     */
     static int InsertLocationRuleInfo(const char * ruleName,
                                int iLocationID,
                                double dLocationLon,
                                double dLocationLat,
-                               double dLocationHeight,
+                               float dLocationHeight,
                                float fLocationSpeed
                                );
 
+
+    /**
+    * @brief 向location_rule_info表中插入一条 action_Directive表中插入一条
+    * @return 0失败
+    */
+    static int InsertLocationRuleInfoAndActionDirective(const char* ruleName,
+                                                        int iDirectiveID,
+                                                        const char* directiveInfo,
+                                                        int iDirectiveResult,
+                                                        int iLocationID,
+                                                        double dLocationLon,
+                                                        double dLocationLat,
+                                                        float dLocationHeight,
+                                                        float fLocationSpeed);
+
     /**
     * @brief 在action_firective表中,
     * @brief 删除rule_name的记录

src/decision/package.xml

@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>desision</name>
+  <name>decision</name>
   <version>0.1.0</version>
-  <description>The desision package</description>
+  <description>The decision package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
@@ -50,6 +50,10 @@
   <!-- Use doc_depend for packages you need only for building documentation: -->
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp message_generation std_msgs rospy</build_depend>
+  <build_export_depend>roscpp message_generation std_msgs rospy</build_export_depend>
+  <exec_depend>roscpp message_generation std_msgs rospy</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

src/decision/srv/CommandModeCmd.srv

@@ -0,0 +1,3 @@
+int32 Cmd
+---
+int32 CmdResult

src/perception/CMakeLists.txt

@@ -219,7 +219,7 @@ add_executable(perception
     perception.h
     perception.cpp
  )
-
+add_dependencies(perception ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(perception
     ${catkin_LIBRARIES}
     glog