agv_init 20200731

.gitignore

@@ -0,0 +1,36 @@
+
+#Ignore thumbnails created by Windows
+Thumbs.db
+#Ignore files built by Visual Studio
+*.obj
+*.exe
+*.pdb
+*.user
+*.aps
+*.pch
+*.vspscc
+*_i.c
+*_p.c
+*.ncb
+*.suo
+*.tlb
+*.tlh
+*.bak
+*.cache
+*.ilk
+*.log
+[Bb]in
+[Dd]ebug*/
+*.lib
+*.sbr
+obj/
+[Rr]elease*/
+_ReSharper*/
+[Tt]est[Rr]esult*
+.vs/
+#Nuget packages folder
+packages/
+build/*
+devel/*
+*vscode*
+.catkin_workspace

src/CMakeLists.txt

@@ -0,0 +1 @@
+/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake

src/MPC/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(MPC)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+#add_compile_options(-std=c++11)
+set(CMAKE_CXX_STANDARD 11)
+set(CMAKE_BUILD_TYPE "RELEASE")
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+        nav_msgs
+        tf
+)
+
+catkin_package(
+
+)
+
+include_directories(
+        # include
+        "/usr/include/eigen3"
+        ${catkin_INCLUDE_DIRS}
+)
+link_directories(/usr/local/lib)
+add_executable(${PROJECT_NAME}_node src/node.cpp src/trajectory/make_trajectory.cpp
+        src/mpc/mpc_tools.cpp src/mpc/MonitorMPC.cpp src/trajectory/FourthTrajectory.cpp
+        src/mpc/TowBotMPC.cpp)
+target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ipopt)
+
+
+add_executable(${PROJECT_NAME}2_node src/node2.cpp src/trajectory/make_trajectory.cpp
+        src/mpc/mpc_tools.cpp src/mpc/MonitorMPC.cpp src/trajectory/FourthTrajectory.cpp
+        src/mpc/TowBotMPC.cpp)
+target_link_libraries(${PROJECT_NAME}2_node ${catkin_LIBRARIES} ipopt)

src/MPC/launch/MPC.launch

@@ -0,0 +1,9 @@
+<launch>
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find MPC)/maps/blank_map.yaml"/>
+  <node pkg="MPC" type="MPC_node" name="MPC_node" output="screen"/>
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find MPC)/launch/default.rviz" />
+
+  <node pkg="turtlesim" name="sim" type="turtlesim_node"/>
+
+
+</launch>

src/MPC/launch/MPC_2.launch

@@ -0,0 +1,15 @@
+<launch>
+
+  <group ns="turtlesim1">
+     <node pkg="turtlesim" name="sim" type="turtlesim_node"/>
+   </group>
+
+  <group ns="turtlesim2">
+    <node pkg="turtlesim" name="sim" type="turtlesim_node"/>
+  </group>
+
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/blank_map.yaml"/>
+  <node pkg="MPC" type="MPC2_node" name="MPC2_node" output="screen"/>
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find MPC)/launch/default.rviz" />
+
+</launch>

src/MPC/maps/blank_map.yaml

@@ -0,0 +1,6 @@
+image: blank_map.pgm
+resolution: 0.02
+origin: [-2.5, -2.5, 0]
+occupied_thresh: 0.65
+free_thresh: 0.196 # Taken from the Willow Garage map in the turtlebot_navigation package
+negate: 0

src/MPC/package.xml

@@ -0,0 +1,70 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>MPC</name>
+  <version>0.0.0</version>
+  <description>The MPC package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="zx@todo.todo">zx</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/MPC</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/MPC/src/mpc/MonitorMPC.cpp

@@ -0,0 +1,297 @@
+#include "MonitorMPC.h"
+#include <cppad/cppad.hpp>
+#include <cppad/ipopt/solve.hpp>
+#include "Eigen/Core"
+#include "Eigen/QR"
+
+using CppAD::AD;
+
+// Set the timestep length and duration
+// Currently tuned to predict 1 second worth
+size_t N = 10;
+
+
+// The solver takes all the state variables and actuator
+// variables in a singular vector. Thus, we should to establish
+// when one variable starts and another ends to make our lifes easier.
+size_t nx = 0;
+size_t ny = nx + N;
+size_t nth = ny + N;
+size_t nv = nth + N;
+size_t nvth = nv + N;
+size_t nacc = nvth + N;
+size_t naccth = nacc + N - 1;
+
+class FG_eval {
+public:
+    // Fitted polynomial coefficients
+    Eigen::VectorXd coeffs;
+    double m_ref_v,m_dt;
+    double m_v,m_vth;
+    FG_eval(Eigen::VectorXd coeffs,double v,double vth,double ref_v,double dt) {
+        this->coeffs = coeffs;
+        m_v=v;
+        m_vth=vth;
+        m_ref_v=ref_v;
+        m_dt=dt;
+    }
+
+    typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
+    void operator()(ADvector& fg, const ADvector& vars) {
+        // Implementing MPC below
+        // `fg` a vector of the cost constraints, `vars` is a vector of variable values (state & actuators)
+        // The cost is stored is the first element of `fg`.
+        // Any additions to the cost should be added to `fg[0]`.
+        fg[0] = 0;
+        double dt=m_dt;
+        // Reference State Cost
+        // Below defines the cost related the reference state and
+        // any anything you think may be beneficial.
+
+        // Weights for how "important" each cost is - can be tuned
+        const int y_cost_weight = 2500;
+        const int th_cost_weight = 1000;
+        const int v_cost_weight = 1;
+        const int vth_cost_weight = 10;
+        const int a_cost_weight = 1;
+        const int ath_cost_weight=10;
+
+        // Cost for CTE, psi error and velocity
+        for (int t = 0; t < N; t++)
+        {
+            // y坐标差
+            AD<double> xt = vars[nx + t];
+            AD<double> fx = coeffs[0] + coeffs[1] * xt + coeffs[2] * pow(xt, 2) + coeffs[3] * pow(xt, 3);
+            fg[0] += y_cost_weight * CppAD::pow(vars[ny + t] - fx, 2);
+
+            // theta, v, vth cost
+            AD<double> fth = CppAD::atan(coeffs[1] + 2 * coeffs[2] * xt + 3 * coeffs[3] * pow(xt, 2));
+            fg[0] += th_cost_weight * CppAD::pow(vars[nth + t] - fth, 2);
+            fg[0] += v_cost_weight * CppAD::pow(vars[nv + t] - m_ref_v, 2);
+            fg[0] += vth_cost_weight * CppAD::pow(vars[nvth + t], 2);
+        }
+
+        // Costs for steering (delta) and acceleration (a)
+        for (int t = 0; t < N - 1; t++)
+        {
+            // a, ath cost
+            fg[0] += a_cost_weight * CppAD::pow(vars[nv + t + 1] - vars[nv + t], 2);
+            fg[0] += ath_cost_weight * CppAD::pow(vars[nvth + t + 1] - vars[nvth + t], 2);
+        }
+
+        /////////////////////
+        // x, y, theta路径约束
+        fg[1 + nx] = vars[nx];// - vars[nv] * dt;
+        fg[1 + ny] = vars[ny];
+        fg[1 + nth] = vars[nth];// - vars[nvth] * dt;
+        fg[1 + nv] = vars[nv];
+        fg[1 + nvth] = vars[nvth];
+
+        // The rest of the constraints
+        for (int t = 1; t < N; t++)
+        {
+            // State at time t + 1
+            AD<double> x1 = vars[nx + t];
+            AD<double> y1 = vars[ny + t];
+            AD<double> th1 = vars[nth + t];
+            AD<double> v1 = vars[nv + t];
+            AD<double> vth1 = vars[nvth + t];
+
+            // State at time t
+            AD<double> x0 = vars[nx + t - 1];
+            AD<double> y0 = vars[ny + t - 1];
+            AD<double> th0 = vars[nth + t - 1];
+            AD<double> v0 = vars[nv + t - 1];
+            AD<double> vth0 = vars[nvth + t - 1];
+
+            AD<double> acc0 = vars[nacc + t - 1];
+            AD<double> accth0 = vars[naccth + t -1];
+
+            // Actuator constraints at time t only
+
+            // Setting up the rest of the model constraints
+            fg[1 + nx + t] = x1 - (x0 + v0 * CppAD::cos(th0) * dt);
+            fg[1 + ny + t] = y1 - (y0 + v0 * CppAD::sin(th0) * dt);
+            fg[1 + nth + t] = th1 - (th0 + vth0 * dt);
+            fg[1 + nv + t] = v1 - (v0 + acc0 * dt);
+            fg[1 + nvth + t] = vth1 - (vth0 + accth0 * dt);
+        }
+
+        // fg[1 + nv] = (vars[nv] - m_v) / dt;
+        // fg[1 + nvth] = (vars[nvth] - m_vth) / dt;
+        // for (int t = 1; t < N; ++t)
+        // {
+        //     fg[1 + nv + t] = (vars[nv + t] - vars[nv + t - 1]) / dt;
+        //     fg[1 + nvth + t] = (vars[nvth + t] - vars[nvth + t - 1]) / dt;
+        // }
+
+        ////  add
+    }
+};
+
+//
+// MPC class definition implementation.
+//
+MonitorMPC::MonitorMPC() {}
+MonitorMPC::~MonitorMPC() {}
+
+bool MonitorMPC::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs, std::vector<double>& out) {
+    bool ok = true;
+    typedef CPPAD_TESTVECTOR(double) Dvector;
+
+    // State vector holds all current values neede for vars below
+    double v = state[0];
+    double vth = state[1];
+    double ref_v = state[2];
+    double max_v=state[3];
+    double dt=state[4];
+
+    // std::cout<<"params: "<<state.transpose()<<std::endl;
+    // Setting the number of model variables (includes both states and inputs).
+    // N * state vector size + (N - 1) * 2 actuators (For steering & acceleration)
+    size_t n_vars = N * 7 - 2;
+    // Setting the number of constraints
+    size_t n_constraints = N * 5;
+
+    // Initial value of the independent variables.
+    // SHOULD BE 0 besides initial state.
+    Dvector vars(n_vars);
+    for (int i = 0; i < n_vars; i++) {
+        vars[i] = 0.0;
+    }
+
+    for (int i = 0; i < N; ++i)
+    {
+        vars[nv + i] = v;
+        vars[nvth + i] = vth;
+    }
+    // vars[nv] = v;
+    // vars[nvth] = vth;
+
+    Dvector vars_lowerbound(n_vars);
+    Dvector vars_upperbound(n_vars);
+    // Sets lower and upper limits for variables.
+    // Set all non-actuators upper and lowerlimits
+    // to the max negative and positive values.
+    for (int i = 0; i < n_vars; i++) {
+        vars_lowerbound[i] = -1.0e19;
+        vars_upperbound[i] = 1.0e19;
+    }
+
+    //// limit  v
+    for (int i = nv; i < nv+N; i++) {
+        vars_lowerbound[i] = -max_v;
+        vars_upperbound[i] = max_v;
+    }
+    // vars_lowerbound[nv] = v-0.01;
+    // vars_upperbound[nv] = v+0.01;
+
+    ////limint vth
+    for (int i = nvth; i < nvth+N; i++) {
+        vars_lowerbound[i] = -0.03;
+        vars_upperbound[i] = 0.03;
+    }
+    // vars_lowerbound[nvth] = vth-0.01;
+    // vars_upperbound[nvth] = vth+0.01;
+
+    // acc
+    for (int i = nacc; i < nacc+N-1; i++) {
+        vars_lowerbound[i] = -5;
+        vars_upperbound[i] = 5;
+    }
+
+    // acc th
+    for (int i = naccth; i < naccth+N-1; i++) {
+        vars_lowerbound[i] = -0.5;
+        vars_upperbound[i] = 0.5;
+    }
+
+    // Lower and upper limits for the constraints
+    // Should be 0 besides initial state.
+    Dvector constraints_lowerbound(n_constraints);
+    Dvector constraints_upperbound(n_constraints);
+    for (int i = 0; i < n_constraints; i++) {
+        constraints_lowerbound[i] = 0;
+        constraints_upperbound[i] = 0;
+    }
+    // 对传入v vth进行限制
+    if(v < vars_lowerbound[nv])
+        v = vars_lowerbound[nv];
+    if(v > vars_upperbound[nv])
+        v = vars_upperbound[nv];
+
+    if(vth < vars_lowerbound[nvth])
+        vth = vars_lowerbound[nvth];
+    if(vth > vars_upperbound[nvth])
+        vth = vars_upperbound[nvth];
+    // v vth初值约束
+    constraints_lowerbound[nv] = v;
+    constraints_upperbound[nv] = v;
+    constraints_lowerbound[nvth] = vth;
+    constraints_upperbound[nvth] = vth;
+	
+    // // acc v
+    // for (int i = nv; i < nv+N; i++) {
+    //     constraints_lowerbound[i] = -0.001;
+    //     constraints_upperbound[i] = 0.001;
+    // }
+    // //// acc vth
+    // for (int i = nvth; i < nvth+N; i++) {
+    //     constraints_lowerbound[i] = -0.001;
+    //     constraints_upperbound[i] = 0.001;
+    // }
+
+
+    // object that computes objective and constraints
+    FG_eval fg_eval(coeffs,v,vth,ref_v,dt);
+
+    //
+    // NOTE: You don't have to worry about these options
+    //
+    // options for IPOPT solver
+    std::string options;
+    // Uncomment this if you'd like more print information
+    options += "Integer print_level  0\n";
+    // NOTE: Setting sparse to true allows the solver to take advantage
+    // of sparse routines, this makes the computation MUCH FASTER. If you
+    // can uncomment 1 of these and see if it makes a difference or not but
+    // if you uncomment both the computation time should go up in orders of
+    // magnitude.
+    options += "Sparse  true        forward\n";
+    options += "Sparse  true        reverse\n";
+    // NOTE: Currently the solver has a maximum time limit of 0.5 seconds.
+    // Change this as you see fit.
+    options += "Numeric max_cpu_time          0.5\n";
+
+    // place to return solution
+    CppAD::ipopt::solve_result<Dvector> solution;
+
+    // solve the problem
+    CppAD::ipopt::solve<Dvector, FG_eval>(
+        options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
+        constraints_upperbound, fg_eval, solution);
+
+    // Check some of the solution values
+    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
+
+    // Cost
+    auto cost = solution.obj_value;
+    //std::cout << "Cost " << cost << std::endl;
+
+    // Return the first actuator values, along with predicted x and y values to plot in the simulator.
+    out.clear();
+    m_trajectory.clear();
+    out.push_back(solution.x[nv+1]);
+    out.push_back(solution.x[nvth+1]);
+    for (int i = 0; i < N; ++i) {
+        out.push_back(solution.x[nx + i]);
+        out.push_back(solution.x[ny + i]);
+        tf::Pose pose;
+        pose.setOrigin(tf::Vector3(solution.x[nx+i],solution.x[ny+i],0));
+        m_trajectory.push_back(pose);
+    }
+    //printf(" cost : %.3f\n",cost);
+    // printf("v0 vth0 v1 vth1 acc0 accth1: %.3f, %.3f, %.3f, %.3f, %.3f, %.3f\n",solution.x[nv],solution.x[nvth],solution.x[nv+1],solution.x[nvth],solution.x[nacc],solution.x[naccth]);
+    return ok;
+
+}

src/MPC/src/mpc/MonitorMPC.h

@@ -0,0 +1,24 @@
+//
+// Created by zx on 2019/8/28.
+//
+
+#ifndef MONITORMPC_H
+#define MONITORMPC_H
+
+#include <vector>
+#include <Eigen/Core>
+#include <tf/transform_datatypes.h>
+
+class MonitorMPC
+{
+public:
+    MonitorMPC();
+    virtual ~MonitorMPC();
+
+    bool Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs, std::vector<double>& out);
+
+    std::vector<tf::Pose> m_trajectory;
+};
+
+
+#endif //MONITORMPC_H

src/MPC/src/mpc/TowBotMPC.cpp

@@ -0,0 +1,382 @@
+//
+// Created by zx on 2019/9/3.
+//
+
+#include "TowBotMPC.h"
+#include <cppad/cppad.hpp>
+#include <cppad/ipopt/solve.hpp>
+#include "Eigen/Core"
+#include "Eigen/QR"
+
+using CppAD::AD;
+
+size_t K = 10;
+size_t nx1 = 0;
+size_t ny1 = nx1 + K;
+size_t nth1 = ny1 + K;
+size_t nv1 = nth1 + K;
+size_t nvth1 = nv1 + K;
+size_t nacc1 = nvth1 + K;
+size_t naccth1 = nacc1 + K - 1;
+
+size_t nx2 = naccth1 + K - 1;
+size_t ny2 = nx2 + K;
+size_t nth2 = ny2 + K;
+size_t nv2 = nth2 + K;
+size_t nvth2 = nv2 + K;
+size_t nacc2 = nvth2 + K;
+size_t naccth2 = nacc2 + K - 1;
+
+size_t relative_pose_contrains = naccth2 + K - 1;
+
+double Lx1 = -1.3;
+double Ly1 = 0;
+double Lx2 = 1.3;
+double Ly2 = 0;
+
+double ref_dist = 2.0;
+// double min_dist = 1.5;
+// double max_dist = 3.0;
+
+class FG_eval_2
+{
+public:
+    // Fitted polynomial coefficients
+    Eigen::VectorXd coeffs, coeffs_b;
+    Eigen::VectorXd m_data;
+    FG_eval_2(Eigen::VectorXd coeffs, Eigen::VectorXd coeffs_b, Eigen::VectorXd P)
+    {
+        this->coeffs = coeffs;
+        this->coeffs_b = coeffs_b;
+        m_data = P;
+    }
+
+    typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
+    void operator()(ADvector &fg, const ADvector &vars)
+    {
+
+        // std::cout << "max size: " << naccth2 + K - 1 << std::endl;
+        double v1 = m_data[0];
+        double v2 = m_data[1];
+        double vth1 = m_data[2];
+        double vth2 = m_data[3];
+        double ref_v = m_data[4];
+        double dt = m_data[5];
+        
+        /// 两车初始位姿差   前车相对后车坐标系
+        double dx = m_data[6]; 
+        double dy = m_data[7];
+        double dth = m_data[8]; ///两坐标系角度
+
+        ///前车 cost weight
+        const int y_cost_weight = 5000;
+        const int th_cost_weight = 1000;
+        const int v_cost_weight = 20;
+
+        const int vth_cost_weight = 100;
+        const int a_cost_weight = 1;
+        const int ath_cost_weight = 10;
+
+        fg[0] = 0;
+        ///后车 cost weight
+        const int back_y_cost_weight = 5000;
+        const int back_th_cost_weight = 1000;
+        const int back_v_cost_weight = 20;
+
+        const int back_vth_cost_weight = 100;
+        const int back_a_cost_weight = 1;
+        const int back_ath_cost_weight = 10;
+
+        for (int t = 0; t < K; t++)
+        {
+            // 前车坐标系下，y坐标差
+            AD<double> xt = vars[nx1 + t];
+            AD<double> fx = coeffs[0] + coeffs[1] * xt + coeffs[2] * pow(xt, 2) + coeffs[3] * pow(xt, 3);
+            fg[0] += y_cost_weight * CppAD::pow(vars[ny1 + t] - fx, 2);
+
+            // 前车坐标系下，theta cost & v cost
+            AD<double> fth = CppAD::atan(coeffs[1] + 2 * coeffs[2] * xt + 3 * coeffs[3] * pow(xt, 2));
+            fg[0] += th_cost_weight * CppAD::pow(vars[nth1 + t] - fth, 2);
+            fg[0] += v_cost_weight * CppAD::pow(vars[nv1 + t] - ref_v, 2);
+            fg[0] += vth_cost_weight * CppAD::pow(vars[nvth1 + t], 2);
+
+            // 在前车坐标系下，后车y坐标差
+            AD<double> xt2 = vars[nx2 + t];
+            AD<double> fx2 = coeffs_b[0] + coeffs_b[1] * xt2 + coeffs_b[2] * pow(xt2, 2) + coeffs_b[3] * pow(xt2, 3);
+            fg[0] += back_y_cost_weight * CppAD::pow(vars[ny2 + t] - fx2, 2);
+
+            // 在前车坐标系下，后车theta cost & v cost
+            AD<double> fth2 = CppAD::atan(coeffs_b[1] + 2 * coeffs_b[2] * xt2 + 3 * coeffs_b[3] * pow(xt2, 2));
+            fg[0] += back_th_cost_weight * CppAD::pow(vars[nth2 + t] - fth2, 2);
+            fg[0] += back_v_cost_weight * CppAD::pow(vars[nv2 + t] - ref_v, 2);
+            fg[0] += back_vth_cost_weight * CppAD::pow(vars[nvth2 + t], 2);
+        }
+        //加速度 cost
+        // Costs for steering (delta) and acceleration (a)
+        for (int t = 0; t < K - 1; t++)
+        {
+            fg[0] += a_cost_weight * CppAD::pow(vars[nacc1+ t], 2);
+            fg[0] += ath_cost_weight * CppAD::pow(vars[naccth1 + t], 2);
+
+            fg[0] += back_a_cost_weight * CppAD::pow(vars[nacc2 + t], 2);
+            fg[0] += back_ath_cost_weight * CppAD::pow(vars[naccth2 + t], 2);
+        }
+
+        ///添加两车距离损失
+        const int distance_weight = 800;
+        const int delta_th_weight = 10;
+        char buf[255]={0};
+        AD<double> dtheta = vars[nth1] - vars[nth2] + dth;
+        for (int t = 0; t < K; ++t)
+        {
+            AD<double> x1_in_front = vars[nx1 + t];
+            AD<double> y1_in_front = vars[ny1 + t];
+            ///将坐标旋转到后车坐标系下
+            AD<double> x1_in_back = CppAD::cos(dtheta) * x1_in_front - CppAD::sin(dtheta) * y1_in_front + dx;
+            AD<double> y1_in_back = CppAD::sin(dtheta) * x1_in_front + CppAD::cos(dtheta) * y1_in_front + dy;
+
+            AD<double> distance_x = x1_in_back - vars[nx2 + t];
+            AD<double> distance_y = y1_in_back - vars[ny2 + t];
+
+            fg[0] += distance_weight * CppAD::pow(CppAD::sqrt(CppAD::pow(distance_x, 2) + CppAD::pow(distance_y, 2)) - ref_dist, 2);
+            fg[0] += delta_th_weight * CppAD::pow(CppAD::atan(y1_in_back / x1_in_back), 2);
+            //sprintf(buf+strlen(buf), " %.4f ", sqrt(pow(distance_x,2)+pow(distance_y,2)));
+            // // 两车最短距离约束
+            // fg[1 + relative_pose_contrains + t - 4*(K-1)] = CppAD::sqrt(CppAD::pow(distance_x, 2) + CppAD::pow(distance_y, 2));
+        }
+        //std::cout<<buf<<std::endl;
+
+        // x, y, theta 路径约束
+        fg[1 + nx1] = vars[nx1];// - vars[nv1] * dt;
+        fg[1 + ny1] = vars[ny1];
+        fg[1 + nth1] = vars[nth1];// - vars[nvth1] * dt;
+        fg[1 + nv1] = vars[nv1];
+        fg[1 + nvth1] = vars[nvth1];
+
+        fg[1 + nx2 - 2*(K-1)] = vars[nx2];// - vars[nv2] * dt;
+        fg[1 + ny2 - 2*(K-1)] = vars[ny2];
+        fg[1 + nth2 - 2*(K-1)] = vars[nth2];// - vars[nvth2] * dt;
+        fg[1 + nv2 - 2*(K-1)] = vars[nv2];
+        fg[1 + nvth2 - 2*(K-1)] = vars[nvth2];
+
+        // The rest of the constraints
+        for (int t = 1; t < K; t++)
+        {
+            /// 位置约束 1
+            AD<double> fx1 = vars[nx1 + t];
+            AD<double> fy1 = vars[ny1 + t];
+            AD<double> fth1 = vars[nth1 + t];
+            AD<double> fv1 = vars[nv1 + t];
+            AD<double> fvth1 = vars[nvth1 + t];
+
+            AD<double> fx0 = vars[nx1 + t - 1];
+            AD<double> fy0 = vars[ny1 + t - 1];
+            AD<double> fth0 = vars[nth1 + t - 1];
+            AD<double> fv0 = vars[nv1 + t - 1];
+            AD<double> fvth0 = vars[nvth1 + t - 1];
+
+            AD<double> facc0 = vars[nacc1 + t - 1];
+            AD<double> faccth0 = vars[naccth1 + t - 1];
+
+            fg[1 + nx1 + t] = fx1 - (fx0 + fv0 * CppAD::cos(fth0) * dt);
+            fg[1 + ny1 + t] = fy1 - (fy0 + fv0 * CppAD::sin(fth0) * dt);
+            fg[1 + nth1 + t] = fth1 - (fth0 + fvth0 * dt);
+            fg[1 + nv1 + t] = fv1 - (fv0 + facc0 * dt);
+            fg[1 + nvth1 + t] = fvth1 - (fvth0 + faccth0 * dt);
+
+            //位置约束2
+            AD<double> bx1 = vars[nx2 + t];
+            AD<double> by1 = vars[ny2 + t];
+            AD<double> bth1 = vars[nth2 + t];
+            AD<double> bv1 = vars[nv2 + t];
+            AD<double> bvth1 = vars[nvth2 + t];
+
+            AD<double> bx0 = vars[nx2 + t - 1];
+            AD<double> by0 = vars[ny2 + t - 1];
+            AD<double> bth0 = vars[nth2 + t - 1];
+            AD<double> bv0 = vars[nv2 + t - 1];
+            AD<double> bvth0 = vars[nvth2 + t - 1];
+
+            AD<double> bacc0 = vars[nacc2 + t - 1];
+            AD<double> baccth0 = vars[naccth2 + t - 1];
+
+            fg[1 + nx2 + t - 2*(K-1)] = bx1 - (bx0 + bv0 * CppAD::cos(bth0) * dt);
+            fg[1 + ny2 + t - 2*(K-1)] = by1 - (by0 + bv0 * CppAD::sin(bth0) * dt);
+            fg[1 + nth2 + t - 2*(K-1)] = bth1 - (bth0 + bvth0 * dt);
+            fg[1 + nv2 + t - 2*(K-1)] = bv1 - (bv0 + bacc0 * dt);
+            fg[1 + nvth2 + t - 2*(K-1)] = bvth1 - (bvth0 + baccth0 * dt);
+        }
+    }
+};
+
+bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeffs, Eigen::VectorXd coeffs_b, std::vector<double> &out)
+{
+    bool ok = true;
+    typedef CPPAD_TESTVECTOR(double) Dvector;
+    // State vector holds all current values neede for vars below
+
+    // std::cout<<"state: "<<state<<std::endl;
+    double v1 = state[0];
+    double v2 = state[1];
+    double vth1 = state[2];
+    double vth2 = state[3];
+    double ref_v = state[4];
+    double dt = state[5];
+
+    // double dx = state[6]; /// 两车初始位姿差   前车相对后车坐标系
+    // double dy = state[7];
+    // double dth = state[8]; ///两坐标系角度
+
+    size_t n_vars = 2 * (K * 5 + (K - 1) * 2);
+    size_t n_constraints = K * (5 * 2 + 0);
+
+    Dvector vars(n_vars);
+    for (int i = 0; i < n_vars; i++)
+    {
+        vars[i] = 0.0;
+    }
+    for (int i = 0; i < K; ++i)
+    {
+        vars[nv1 + i] = v1;
+        vars[nv2 + i] = v2;
+
+        vars[nvth1 + i] = vth1;
+        vars[nvth2 + i] = vth2;
+    }
+
+    Dvector vars_lowerbound(n_vars);
+    Dvector vars_upperbound(n_vars);
+
+    for (int i = 0; i < n_vars; i++)
+    {
+        vars_lowerbound[i] = -1.0e19;
+        vars_upperbound[i] = 1.0e19;
+    }
+
+    for (int i = 0; i < K; i++)
+    {
+        //// limit  v1 v2
+        vars_lowerbound[nv1 + i] = -max_v;
+        vars_upperbound[nv1 + i] = max_v;
+
+        vars_lowerbound[nv2 + i] = -max_v;
+        vars_upperbound[nv2 + i] = max_v;
+
+        //// limit  vth1 2
+        vars_lowerbound[nvth1 + i] = -0.5;
+        vars_upperbound[nvth1 + i] = 0.5;
+
+        vars_lowerbound[nvth2 + i] = -0.5;
+        vars_upperbound[nvth2 + i] = 0.5;
+    }
+    for (int i = 0; i < K - 1; i++)
+    {
+        //// limit acc1 acc2
+        vars_lowerbound[nacc1 + i] = -5.0;
+        vars_upperbound[nacc2 + i] = 5.0;
+
+        //// limit accth1 accth2
+        vars_lowerbound[naccth1 + i] = -0.05;//3度
+        vars_upperbound[naccth2 + i] = 0.05;
+    }
+    
+
+    ////   constraints
+    Dvector constraints_lowerbound(n_constraints);
+    Dvector constraints_upperbound(n_constraints);
+    for (int i = 0; i < n_constraints; i++)
+    {
+        constraints_lowerbound[i] = 0;
+        constraints_upperbound[i] = 0;
+    }
+
+    for (int i = 1; i < K; i++)
+    {
+        ////acc v1 v2
+        constraints_lowerbound[nv1 + i] = -3.0;
+        constraints_upperbound[nv1 + i] = 3.0;
+        constraints_lowerbound[nv2 + i - 2*(K-1)] = -3.0;
+        constraints_upperbound[nv2 + i - 2*(K-1)] = 3.0;
+
+        //acc th 1 2
+        constraints_lowerbound[nvth1 + i] = -0.1;
+        constraints_upperbound[nvth1 + i] = 0.1;
+        constraints_lowerbound[nvth2 + i - 2*(K-1)] = -0.1;
+        constraints_upperbound[nvth2 + i - 2*(K-1)] = 0.1;
+
+        // constraints_lowerbound[relative_pose_contrains + i - 4*(K-1)] = min_dist;
+        // constraints_upperbound[relative_pose_contrains + i - 4*(K-1)] = max_dist;
+    }
+    // acc v1 v2
+    constraints_lowerbound[nv1]=v1-0.1;
+    constraints_upperbound[nv1]=v1+0.1;
+    constraints_lowerbound[nv2-2*(K-1)]=v2-0.1;
+    constraints_upperbound[nv2-2*(K-1)]=v2+0.1;
+
+    //acc th1 th2
+    constraints_lowerbound[nvth1]=vth1-0.017;
+    constraints_upperbound[nvth1]=vth1+0.017;
+    constraints_lowerbound[nvth2-2*(K-1)]=vth2-0.1;
+    constraints_upperbound[nvth2-2*(K-1)]=vth2+0.1;
+
+    // object that computes objective and constraintsv
+    FG_eval_2 fg_eval(coeffs, coeffs_b, state);
+
+    std::string options;
+    options += "Integer print_level  0\n";
+    options += "Sparse  true        forward\n";
+    options += "Sparse  true        reverse\n";
+    options += "Numeric max_cpu_time          0.5\n";
+
+    CppAD::ipopt::solve_result<Dvector> solution;
+
+    CppAD::ipopt::solve<Dvector, FG_eval_2>(
+        options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
+        constraints_upperbound, fg_eval, solution);
+
+    // Check some of the solution values
+    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
+
+    // Cost
+    auto cost = solution.obj_value;
+
+    // Return the first actuator values, along with predicted x and y values to plot in the simulator.
+    out.clear();
+    m_trajectory_f.clear();
+    m_trajectory_b.clear();
+    float speed1=solution.x[nv1];
+    if(fabs(speed1)<0.1)
+    {
+        speed1=speed1>0?0.1:-0.1;
+    }
+
+    float speed2=solution.x[nv2];
+    if(fabs(speed2)<0.1)
+    {
+        speed2=speed2>0?0.1:-0.1;
+    }
+    out.push_back(solution.x[nv1]);
+    out.push_back(solution.x[nvth1]);
+    out.push_back(solution.x[nv2]);
+    out.push_back(solution.x[nvth2]);
+    for (int i = 0; i < K; ++i)
+    {
+        tf::Pose pose1, pose2;
+        pose1.setOrigin(tf::Vector3(solution.x[nx1 + i], solution.x[ny1 + i], 0));
+        pose2.setOrigin(tf::Vector3(solution.x[nx2 + i], solution.x[ny2 + i], 0));
+        m_trajectory_f.push_back(pose1);
+        m_trajectory_b.push_back(pose2);
+    }
+
+    // printf("Cost : %.3f ref:%.3f  cv : %.3f  cvth : %.3f  ||  v2:%.3f  th2:%.3f\n",solution.obj_value,
+    //     ref_v,v2,vth2,solution.x[nv2],solution.x[nvth2]);
+
+    return ok;
+}
+
+TowBotMPC::TowBotMPC()
+{
+}
+TowBotMPC::~TowBotMPC()
+{
+}

src/MPC/src/mpc/TowBotMPC.h

@@ -0,0 +1,25 @@
+//
+// Created by zx on 2019/9/3.
+//
+
+#ifndef TOWBOTMPC_H
+#define TOWBOTMPC_H
+#include <vector>
+#include <Eigen/Core>
+#include <tf/transform_datatypes.h>
+
+class TowBotMPC
+{
+public:
+    TowBotMPC();
+    ~TowBotMPC();
+    bool Solve(Eigen::VectorXd state, double max_v,Eigen::VectorXd coeffs,
+        Eigen::VectorXd coeffs_b, std::vector<double>& out);
+
+    std::vector<tf::Pose> m_trajectory_f;
+    std::vector<tf::Pose> m_trajectory_b;
+
+};
+
+
+#endif //TOWBOTMPC_H

src/MPC/src/mpc/mpc_tools.cpp

@@ -0,0 +1,93 @@
+#include  "mpc_tools.h"
+
+constexpr double pi() { return M_PI; }
+double deg2rad(double x) { return x * pi() / 180; }
+double rad2deg(double x) { return x * 180 / pi(); }
+
+
+mpc_tools::mpc_tools()
+{
+}
+
+mpc_tools::~mpc_tools()
+{
+}
+
+//获取小车坐标系中，x前进方向 num 个点，用于拟合
+bool mpc_tools::filterPath(tf::Pose pose, std::vector<tf::Pose> poses, std::vector<tf::Pose> &out,
+                           int numOfPoints, int &index, bool direct)
+{
+  if (poses.size() < numOfPoints)
+    return false;
+
+  out.clear();
+  // 平移加反向旋转到小车坐标系
+  double theta, a, b;
+  pose.getBasis().getEulerZYX(theta, a, b);
+  double min_dist = 1000;
+  int min_index = -1;
+  if (direct)
+  {
+    // std::cout<<"反向"<<std::endl;
+    theta += M_PI;
+  }
+
+  for (int i = 0; i < poses.size(); i++)
+  {
+    // std::cout<<"正向"<<std::endl;
+    double x = poses[i].getOrigin().getX() - pose.getOrigin().getX();
+    double y = poses[i].getOrigin().getY() - pose.getOrigin().getY();
+    double trans_x = x * cos(-theta) - y * sin(-theta);
+    double trans_y = x * sin(-theta) + y * cos(-theta);
+    if (trans_x >= 0)
+    {
+      // tf::Pose point;
+      // float nx = trans_x * cos(theta) - trans_y * sin(theta);
+      // float ny = trans_x * sin(theta) + trans_y * cos(theta);
+      // point.setOrigin(tf::Vector3(nx + pose.getOrigin().getX(),
+      //                             ny + pose.getOrigin().getY(), 0));
+      if (hypot(trans_x, trans_y) < min_dist)
+      {
+        min_dist = hypot(trans_x, trans_y);
+        min_index = i;
+      }
+    }
+  }
+  index = min_index;
+  for (size_t i = min_index; i < poses.size() && out.size() < numOfPoints; i++)
+  {
+    out.push_back(poses[i]);
+  }
+  return true;
+}
+
+// 求y
+double mpc_tools::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;
+}
+
+// 求参数
+Eigen::VectorXd mpc_tools::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;
+}
+

src/MPC/src/mpc/mpc_tools.h

@@ -0,0 +1,29 @@
+#ifndef MPC_TOOLS_HH
+#define MPC_TOOLS_HH 
+
+#include <vector>
+#include <Eigen/Core>
+#include <Eigen/QR>
+#include <tf/transform_datatypes.h>
+
+
+// For converting back and forth between radians and degrees.
+
+
+class mpc_tools{
+
+public:
+    mpc_tools();
+    ~mpc_tools();
+
+    Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order);
+
+    double polyeval(Eigen::VectorXd coeffs, double x);
+
+    bool filterPath(tf::Pose pose,std::vector<tf::Pose> poses, std::vector<tf::Pose>& out,
+        int numOfPoints, int &index, bool derect=false);
+
+
+};
+
+#endif /* MPC_TOOLS_HH */

src/MPC/src/node.cpp

@@ -0,0 +1,333 @@
+//
+// Created by zx on 2019/8/26.
+//
+
+#include <iostream>
+#include <tf/transform_datatypes.h>
+#include <nav_msgs/Odometry.h>
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+#include <nav_msgs/Path.h>
+#include <string>
+#include <chrono>
+
+#include "mpc/MonitorMPC.h"
+#include "trajectory/make_trajectory.h"
+#include "trajectory/FourthTrajectory.h"
+#include "mpc/mpc_tools.h"
+
+#include <random>
+
+ros::Publisher g_vel_pub;
+ros::Publisher g_local_plan_pub,g_global_plan_pub,g_fit_plan_pub;
+tf::Pose g_cur_pose,g_goal;
+bool g_goal_valid=false;
+
+FourthTrajectory maker;
+mpc_tools mpc_tool;
+double g_max_vx;
+double g_ref_v;
+
+void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
+{
+    printf("new goal recieve ...  \n");
+    double x=msg->pose.position.x;
+    double y=msg->pose.position.y;
+    double z=msg->pose.position.z;
+    double qx=msg->pose.orientation.x;
+    double qy=msg->pose.orientation.y;
+    double qz=msg->pose.orientation.z;
+    double qw=msg->pose.orientation.w;
+    g_goal.setOrigin(tf::Vector3(x,y,z));
+    tf::Quaternion quat(qx,qy,qz,qw);
+    g_goal.setRotation(quat);
+
+    ///更新轨迹
+
+    if(maker.Make(g_cur_pose,g_goal,40)) {
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(maker.Size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for (unsigned int i = 0; i < maker.Size(); i++) {
+            gui_path.poses[i].pose.position.x = maker[i].getOrigin().getX();
+            gui_path.poses[i].pose.position.y = maker[i].getOrigin().getY();
+            tf::Quaternion quat=maker[i].getRotation();
+
+            gui_path.poses[i].pose.orientation.x = quat.x();
+            gui_path.poses[i].pose.orientation.y = quat.y();
+            gui_path.poses[i].pose.orientation.z = quat.z();
+            gui_path.poses[i].pose.orientation.w = quat.w();
+        }
+
+        g_global_plan_pub.publish(gui_path);
+        g_goal_valid=true;
+
+    }
+
+}
+std::vector<tf::Transform> local_path;
+
+void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
+{
+    // local_path.clear();
+    ROS_INFO_ONCE("odom received!");
+    static tf::TransformListener tf_listen(ros::Duration(10));
+    static double dt=0.05;
+    auto start = std::chrono::system_clock::now();
+
+    double vx = msg->twist.twist.linear.x;
+    double vth = msg->twist.twist.angular.z;
+    std::string frame_id = msg->child_frame_id;
+    std::string tf_error;
+    tf::StampedTransform transform;
+    try{
+        tf_listen.lookupTransform("/map", "/base_link",ros::Time(0), transform);
+        g_cur_pose=transform;
+        
+        // std::random_device seed_device;
+        // std::default_random_engine engine(seed_device());//通过operator（）取得设备产生的随机值
+        // engine.seed(time(0)); // 如果不传进种子的话，该引擎的状态被回置以后，将会产生同样的值
+        // std::uniform_int_distribution<int> distr(-50,50);//10到100，int类型，线性分布
+        // tf::Vector3 translation = g_cur_pose.getOrigin();
+        // g_cur_pose.setOrigin(tf::Vector3(translation.getX()+distr(engine)/100.0, translation.getY()+distr(engine)/100.0, translation.getZ()));
+    }
+    catch (tf::TransformException &ex) {
+        if(g_goal_valid)
+            ROS_ERROR(" tf listen error : %s",ex.what());
+        return;
+    }
+    int fitNum=20;
+    if(g_goal_valid==false || maker.Size()<fitNum ) return ;
+//////判断是否到达目标点
+    double x = transform.getOrigin().getX();
+    double y = transform.getOrigin().getY();
+    double gx = g_goal.getOrigin().getX();
+    double gy = g_goal.getOrigin().getY();
+    double yaw;
+    double theta,a,b;
+    transform.getBasis().getEulerZYX(theta,a,b);
+    g_goal.getBasis().getEulerZYX(yaw, a, b);
+
+    double dx=g_goal.getOrigin().getX()-transform.getOrigin().getX();
+    double dy=g_goal.getOrigin().getY()-transform.getOrigin().getY();
+    double dis_th = fabs(yaw - theta);
+    if (fabs(dx) < 0.05 && fabs(dy) < 0.03 && dis_th < 0.02 && fabs(vx) < 0.05 && fabs(vth) < 0.05)
+    {
+        geometry_msgs::Twist cmd_vel;
+        cmd_vel.linear.x = 0;
+        cmd_vel.linear.y = 0.0;
+        cmd_vel.angular.z = 0;
+        g_vel_pub.publish(cmd_vel);
+
+        g_goal_valid = false;
+        std::cout<<"   到达目标点 !!!!"<<std::endl;
+        return;
+    }
+
+    //////////////////////////////////////////////////   导航 部分  ///////////////////////////////////
+    tf::Vector3 length(dx,dy,0);
+    double direct_x = dx * cos(-theta) - dy * sin(-theta);
+
+    double g_ref_v=length.length()/(g_max_vx*3.0);
+    g_ref_v=std::max(g_ref_v,0.02);
+    g_ref_v=std::min(g_ref_v,g_max_vx);
+    if (direct_x<0) g_ref_v=-g_ref_v;
+
+    int pick_index = 0;
+    ///// 选点
+    std::vector<tf::Pose> poses;
+    if (!mpc_tool.filterPath(transform, maker.trajectory_, poses, fitNum, pick_index, direct_x < 0))
+    {
+        ROS_ERROR(" fit error ...");
+        return;
+    }
+    else
+    {
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(poses.size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for(unsigned int i=0; i < poses.size(); i++)
+        {
+            float x=poses[i].getOrigin().getX();
+            float y=poses[i].getOrigin().getY();
+
+            gui_path.poses[i].pose.position.x=x;
+            gui_path.poses[i].pose.position.y=y;
+            tf::Quaternion quat=poses[i].getRotation();
+            gui_path.poses[i].pose.orientation.x=quat.x();
+            gui_path.poses[i].pose.orientation.y=quat.y();
+            gui_path.poses[i].pose.orientation.z=quat.z();
+            gui_path.poses[i].pose.orientation.w=quat.w();
+        }
+        //std::cout<<"publish "<<mpc.m_trajectory.size()<<std::endl;
+        g_fit_plan_pub.publish(gui_path);
+
+    }
+
+    //////   fit   select points
+    Eigen::VectorXd ptsx_car(poses.size());
+    Eigen::VectorXd ptsy_car(poses.size());
+
+    for (int i = 0; i < poses.size(); i++)
+    {
+        double x = poses[i].getOrigin().getX() - transform.getOrigin().getX();
+        double y = poses[i].getOrigin().getY() - transform.getOrigin().getY();
+        ptsx_car[i] = x * cos(-theta) - y * sin(-theta);
+        ptsy_car[i] = x * sin(-theta) + y * cos(-theta);
+    }
+
+    Eigen::VectorXd coeffs=mpc_tool.polyfit(ptsx_car,ptsy_car,3);
+
+    // // 接近终点修改参考速度
+    // if(local_path.size() > 1 && maker.trajectory_.size()>1){
+    //     double local_dist = 0;
+    //     double global_dist = 0;
+    //     for (size_t i = 1; i < local_path.size(); i++)
+    //     {
+    //         local_dist += hypot(local_path[i].getOrigin().getX() - local_path[i-1].getOrigin().getX(), local_path[i].getOrigin().getY() - local_path[i-1].getOrigin().getY());
+    //     }
+    //     for (size_t i = pick_index; i < maker.trajectory_.size(); i++)
+    //     {
+    //         global_dist += hypot(maker.trajectory_[i].getOrigin().getX() - maker.trajectory_[i-1].getOrigin().getX(), maker.trajectory_[i].getOrigin().getY() - maker.trajectory_[i-1].getOrigin().getY());
+    //     }
+    //     if(local_dist > global_dist)
+    //     {
+    //         g_ref_v *= 0.5;
+    //     }
+    //     std::cout<<"dists: "<<local_dist<<", "<<global_dist<<std::endl;
+    //     // ref_v=length.length()/(g_max_vx*1.5);
+    //     g_ref_v = fabs(g_ref_v);
+    //     g_ref_v=std::max(g_ref_v,0.02);
+    //     g_ref_v=std::min(g_ref_v,g_max_vx);
+    //     if (direct_x<0) g_ref_v=-g_ref_v;
+    // }
+
+
+    Eigen::VectorXd state(5);
+    dt=0.05;
+    state << vx, vth, g_ref_v, g_max_vx, dt;
+    MonitorMPC mpc;
+    std::vector<double> out;
+
+    if(mpc.Solve(state,coeffs,out))
+    {
+        geometry_msgs::Twist cmd_vel;
+        cmd_vel.linear.x = out[0];
+        cmd_vel.linear.y = 0.0;
+        cmd_vel.angular.z = out[1];
+        g_vel_pub.publish(cmd_vel);
+
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(mpc.m_trajectory.size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        local_path.clear();
+        for (unsigned int i = 0; i < mpc.m_trajectory.size(); i++) {
+            float x = mpc.m_trajectory[i].getOrigin().getX();
+            float y = mpc.m_trajectory[i].getOrigin().getY();
+            float rx = x * cos(theta) - y * sin(theta) + transform.getOrigin().getX();
+            float ry = x * sin(theta) + y * cos(theta) + transform.getOrigin().getY();
+            gui_path.poses[i].pose.position.x = rx;
+            gui_path.poses[i].pose.position.y = ry;
+            tf::Quaternion quat = mpc.m_trajectory[i].getRotation();
+            gui_path.poses[i].pose.orientation.x = quat.x();
+            gui_path.poses[i].pose.orientation.y = quat.y();
+            gui_path.poses[i].pose.orientation.z = quat.z();
+            gui_path.poses[i].pose.orientation.w = quat.w();
+            local_path.push_back(mpc.m_trajectory[i]);
+        }
+        //std::cout<<"publish "<<mpc.m_trajectory.size()<<std::endl;
+        g_local_plan_pub.publish(gui_path);
+
+        auto end = std::chrono::system_clock::now();
+        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+        double time=double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
+    
+        printf("input/(download): %.3f(%.3f)  %.3f(%.3f), ref:%.3f,  dt:%.3f\n",vx,out[0],vth,out[1],g_ref_v,time);
+        double sleep_t=(dt-time)*1000.0;
+        if(sleep_t>0)
+            usleep(1000*int(sleep_t));
+    }
+    else{
+        printf("solve failed   input: %.3f  %.3f, \n",vx,vth);
+        //std::cout<<"solve failed."<<std::endl;
+    }
+
+
+}
+void updateGoal(const geometry_msgs::PoseStamped::ConstPtr& msg)
+{
+    printf("update goal recieve ...  \n");
+    double x=g_cur_pose.getOrigin().getX();
+    double y=g_cur_pose.getOrigin().getY();
+    double z=g_cur_pose.getOrigin().getZ();
+    double qx=g_cur_pose.getRotation().getX();
+    double qy=g_cur_pose.getRotation().getY();
+    double qz=g_cur_pose.getRotation().getZ();
+    double qw=g_cur_pose.getRotation().getW();
+    g_goal.setOrigin(tf::Vector3(x+msg->pose.position.x,y,z));
+    tf::Quaternion quat(qx,qy,qz,qw);
+    g_goal.setRotation(quat);
+
+    ///更新轨迹
+    if(maker.Make(g_cur_pose,g_goal,40)) {
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(maker.Size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for (unsigned int i = 0; i < maker.Size(); i++) {
+            gui_path.poses[i].pose.position.x = maker[i].getOrigin().getX();
+            gui_path.poses[i].pose.position.y = maker[i].getOrigin().getY();
+            tf::Quaternion quat=maker[i].getRotation();
+
+            gui_path.poses[i].pose.orientation.x = quat.x();
+            gui_path.poses[i].pose.orientation.y = quat.y();
+            gui_path.poses[i].pose.orientation.z = quat.z();
+            gui_path.poses[i].pose.orientation.w = quat.w();
+        }
+
+        g_global_plan_pub.publish(gui_path);
+        g_goal_valid=true;
+    }
+}
+int main(int argc ,char* argv[])
+{
+    std::cout<<"  input max vx : ";
+    std::cin>>g_max_vx;
+    std::cout<<std::endl;
+    g_ref_v = g_max_vx;
+
+    ros::init(argc, argv, "MPC_node");
+
+    ros::NodeHandle gn;
+
+    ros::Subscriber sub = gn.subscribe("/move_base_simple/goal",1,goalCallback);//move_base_simple
+    ros::Subscriber sub0 = gn.subscribe("/goal",1,updateGoal);
+
+    ros::Subscriber odom_sub_ = gn.subscribe<nav_msgs::Odometry>( "/odom", 1, odomCallback);
+
+    g_vel_pub = gn.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+    g_global_plan_pub = gn.advertise<nav_msgs::Path>("global_plan",1, true);
+    g_local_plan_pub = gn.advertise<nav_msgs::Path>("local_plan", 1);
+    g_fit_plan_pub = gn.advertise<nav_msgs::Path>("fit_plan", 1);
+
+    ros::spin();
+
+    geometry_msgs::Twist cmd_vel;
+    cmd_vel.linear.x = 0;
+    cmd_vel.linear.y = 0.0;
+    cmd_vel.angular.z = 0;
+    g_vel_pub.publish(cmd_vel);
+
+    return 0;
+}

src/MPC/src/node2.cpp

@@ -0,0 +1,488 @@
+//
+// Created by zx on 2019/9/6.
+//
+
+//
+// Created by zx on 2019/8/26.
+//
+
+#include <iostream>
+#include <tf/transform_datatypes.h>
+#include <nav_msgs/Odometry.h>
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+#include <nav_msgs/Path.h>
+#include <string>
+#include <chrono>
+#include <thread>
+#include <turtlesim/Pose.h>
+
+#include "mpc/TowBotMPC.h"
+#include "trajectory/make_trajectory.h"
+#include "trajectory/FourthTrajectory.h"
+#include "mpc/mpc_tools.h"
+#include <tf/transform_broadcaster.h>
+
+ros::Publisher g_vel_pub1,g_vel_pub2;
+ros::Publisher g_pose_pub1,g_pose_pub2;
+ros::Publisher g_local_plan_pub,g_local_plan_pub_b,g_global_plan_pub,g_fit_plan_pub,g_fit_plan_pub_b;
+tf::Pose g_pose_f,g_pose_b,g_goal;
+geometry_msgs::Twist g_cmd_vel_f,g_cmd_vel_b;
+bool g_goal_valid=false;
+int g_direction=1;      //朝向  向前为0  先后为1
+
+FourthTrajectory maker;
+mpc_tools mpc_tool;
+double g_max_vx;
+
+// 目标设置回调
+void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
+{
+    printf("new goal recieve ...  \n");
+    double x=msg->pose.position.x;
+    double y=msg->pose.position.y;
+    double z=msg->pose.position.z;
+    double qx=msg->pose.orientation.x;
+    double qy=msg->pose.orientation.y;
+    double qz=msg->pose.orientation.z;
+    double qw=msg->pose.orientation.w;
+    g_goal.setOrigin(tf::Vector3(x,y,z));
+    tf::Quaternion quat(qx,qy,qz,qw);
+    g_goal.setRotation(quat);
+
+    double distance1 = g_pose_f.getOrigin().distance(g_goal.getOrigin());
+    double distance2 = g_pose_b.getOrigin().distance(g_goal.getOrigin());
+    tf::Pose start_point=distance1>distance2?g_pose_f:g_pose_b;
+    g_direction=distance1>distance2?-1:1;
+    ///更新轨迹
+
+    if(maker.Make(start_point,g_goal,30)) {
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(maker.Size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for (unsigned int i = 0; i < maker.Size(); i++) {
+            gui_path.poses[i].pose.position.x = maker[i].getOrigin().getX();
+            gui_path.poses[i].pose.position.y = maker[i].getOrigin().getY();
+            tf::Quaternion quat=maker[i].getRotation();
+
+            gui_path.poses[i].pose.orientation.x = quat.x();
+            gui_path.poses[i].pose.orientation.y = quat.y();
+            gui_path.poses[i].pose.orientation.z = quat.z();
+            gui_path.poses[i].pose.orientation.w = quat.w();
+        }
+
+        g_global_plan_pub.publish(gui_path);
+        g_goal_valid=true;
+
+    }
+
+}
+
+// 一号车位姿回调
+void poseCallback1(const turtlesim::Pose::ConstPtr& msg)
+{
+    double theta=msg->theta;
+    tf::Quaternion q;
+    q.setRPY(0,0,theta);
+    g_pose_f.setRotation(q);
+    g_pose_f.setOrigin(tf::Vector3(msg->x-2.0,msg->y,0));
+
+    g_cmd_vel_f.linear.x=msg->linear_velocity;
+    g_cmd_vel_f.angular.z=msg->angular_velocity;
+
+    geometry_msgs::PoseStamped pose;
+    pose.header.frame_id="map";
+    pose.header.stamp=ros::Time::now();
+    pose.pose.position.x=g_pose_f.getOrigin().getX();
+    pose.pose.position.y=g_pose_f.getOrigin().getY();
+    pose.pose.position.z=0;
+    pose.pose.orientation.x=q.x();
+    pose.pose.orientation.y=q.y();
+    pose.pose.orientation.z=q.z();
+    pose.pose.orientation.w=q.w();
+    g_pose_pub1.publish(pose);
+
+    static tf::TransformBroadcaster broadcaster;
+    tf::Pose conn_point;
+    conn_point.setOrigin(tf::Vector3(-1.3,0,0));
+    conn_point.setRotation(tf::Quaternion(0,0,0,1));
+    tf::Pose conn=g_pose_f*conn_point;
+
+    broadcaster.sendTransform(
+        tf::StampedTransform(conn,ros::Time::now(),"map", "conn1"));
+
+
+}
+
+// 二号车回调
+void poseCallback2(const turtlesim::Pose::ConstPtr& msg)
+{
+    double theta=msg->theta;
+    tf::Quaternion q;
+    q.setRPY(0,0,theta);
+
+    g_pose_b.setRotation(q);
+    g_pose_b.setOrigin(tf::Vector3(msg->x-5.0,msg->y,0));
+
+    g_cmd_vel_b.linear.x=msg->linear_velocity;
+    g_cmd_vel_b.angular.z=msg->angular_velocity;
+
+    geometry_msgs::PoseStamped pose;
+    pose.header.frame_id="map";
+    pose.header.stamp=ros::Time::now();
+    pose.pose.position.x=g_pose_b.getOrigin().getX();
+    pose.pose.position.y=g_pose_b.getOrigin().getY();
+    pose.pose.position.z=0;
+    pose.pose.orientation.x=q.x();
+    pose.pose.orientation.y=q.y();
+    pose.pose.orientation.z=q.z();
+    pose.pose.orientation.w=q.w();
+    g_pose_pub2.publish(pose);
+
+    static tf::TransformBroadcaster broadcaster;
+    tf::Pose conn_point;
+    conn_point.setOrigin(tf::Vector3(1.3,0,0));
+    conn_point.setRotation(tf::Quaternion(0,0,0,1));
+    tf::Pose conn=g_pose_b*conn_point;
+
+    broadcaster.sendTransform(
+        tf::StampedTransform(conn,ros::Time::now(),"map", "conn2"));
+}
+
+void publish_cmd(geometry_msgs::Twist cmd_vel1,geometry_msgs::Twist cmd_vel2)
+{
+    ////
+    g_vel_pub1.publish(cmd_vel1);
+    g_vel_pub2.publish(cmd_vel2);
+
+}
+
+bool MPC()
+{
+    static tf::TransformListener tf_listen(ros::Duration(10));
+    static double dt = 0.05;
+    auto start = std::chrono::system_clock::now();
+
+    std::string tf_error;
+
+    int fitNum = 6;
+    if (g_goal_valid == false || maker.Size() < fitNum) return false;
+
+//////判断是否到达目标点
+    double yaw;
+    double theta, a, b;
+    g_pose_f.getBasis().getEulerZYX(theta, a, b);
+    g_goal.getBasis().getEulerZYX(yaw, a, b);
+
+    double theta_b;
+    g_pose_b.getBasis().getEulerZYX(theta_b, a, b);
+
+    // 设置线速度、角速度，以及终点判定
+    double dis_th=0,offsetx = 0, offsety = 0,liner_vx=0,angular_vz=0;
+    if (g_direction == 1) {
+        dis_th=fabs(yaw - theta);
+        offsetx = g_goal.getOrigin().getX() - g_pose_f.getOrigin().getX();
+        offsety = g_goal.getOrigin().getY() - g_pose_f.getOrigin().getY();
+        liner_vx=g_cmd_vel_f.linear.x;
+        angular_vz=g_cmd_vel_f.angular.z;
+    }else {
+        dis_th=fabs(yaw - theta_b);
+        offsetx = g_goal.getOrigin().getX() - g_pose_b.getOrigin().getX();
+        offsety = g_goal.getOrigin().getY() - g_pose_b.getOrigin().getY();
+        liner_vx=g_cmd_vel_b.linear.x;
+        angular_vz=g_cmd_vel_b.angular.z;
+    }
+    if (fabs(offsetx) < 0.05 && fabs(offsety) < 0.03 && dis_th < 0.02 && fabs(liner_vx) < 0.05
+        && fabs(angular_vz) < 0.05) {
+        geometry_msgs::Twist cmd_vel;
+        cmd_vel.linear.x = 0;
+        cmd_vel.linear.y = 0.0;
+        cmd_vel.angular.z = 0;
+        publish_cmd(cmd_vel, cmd_vel);
+
+        g_goal_valid = false;
+        std::cout << "   到达目标点 !!!!" << std::endl;
+        return true;
+    }
+
+    //////////////////////////////////////////////////   导航 部分  ///////////////////////////////////
+    double dx=offsetx,dy=offsety;
+    int pick_index = 0;
+
+    tf::Vector3 length(dx, dy, 0);
+    double direct_x = dx * cos(-theta) - dy * sin(-theta);
+    double ref_v = length.length() / (g_max_vx * 1.5);
+    ref_v = std::max(ref_v, 0.02);
+    ref_v = std::min(ref_v, g_max_vx);
+    if (direct_x < 0) ref_v = -ref_v;
+
+    ///// 选点
+    std::vector<tf::Pose> poses;
+    std::vector<tf::Pose> poses_b;
+    if (!mpc_tool.filterPath(g_pose_f, maker.trajectory_, poses, fitNum, pick_index, direct_x < 0)) {
+        ROS_ERROR(" fit error ...");
+        return false;
+    }
+    else {
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(poses.size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for (unsigned int i = 0; i < poses.size(); i++) {
+            float x = poses[i].getOrigin().getX();
+            float y = poses[i].getOrigin().getY();
+
+            gui_path.poses[i].pose.position.x = x;
+            gui_path.poses[i].pose.position.y = y;
+            tf::Quaternion quat = poses[i].getRotation();
+            gui_path.poses[i].pose.orientation.x = quat.x();
+            gui_path.poses[i].pose.orientation.y = quat.y();
+            gui_path.poses[i].pose.orientation.z = quat.z();
+            gui_path.poses[i].pose.orientation.w = quat.w();
+        }
+        //std::cout<<"publish "<<mpc.m_trajectory.size()<<std::endl;
+        g_fit_plan_pub.publish(gui_path);
+    }
+    if (!mpc_tool.filterPath(g_pose_b, maker.trajectory_, poses_b, fitNum, pick_index, direct_x < 0)) {
+        ROS_ERROR(" fit error ...");
+        return false;
+    }
+    else {
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(poses.size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for (unsigned int i = 0; i < poses_b.size(); i++) {
+            float x = poses_b[i].getOrigin().getX();
+            float y = poses_b[i].getOrigin().getY();
+
+            gui_path.poses[i].pose.position.x = x;
+            gui_path.poses[i].pose.position.y = y;
+            tf::Quaternion quat = poses_b[i].getRotation();
+            gui_path.poses[i].pose.orientation.x = quat.x();
+            gui_path.poses[i].pose.orientation.y = quat.y();
+            gui_path.poses[i].pose.orientation.z = quat.z();
+            gui_path.poses[i].pose.orientation.w = quat.w();
+        }
+        //std::cout<<"publish "<<mpc.m_trajectory.size()<<std::endl;
+        g_fit_plan_pub_b.publish(gui_path);
+
+    }
+
+    // 生成参数
+    Eigen::VectorXd ptsx_car(poses.size());
+    Eigen::VectorXd ptsy_car(poses.size());
+    for (int i = 0; i < poses.size(); i++) {
+        double x = poses[i].getOrigin().getX() - g_pose_f.getOrigin().getX();
+        double y = poses[i].getOrigin().getY() - g_pose_f.getOrigin().getY();
+        ptsx_car[i] = x * cos(-theta) - y * sin(-theta);
+        ptsy_car[i] = x * sin(-theta) + y * cos(-theta);
+    }
+    Eigen::VectorXd coeffs = mpc_tool.polyfit(ptsx_car, ptsy_car, 3);
+
+    Eigen::VectorXd ptsx_car_b(poses_b.size());
+    Eigen::VectorXd ptsy_car_b(poses_b.size());
+    for (int i = 0; i < poses_b.size(); i++) {
+        double x = poses_b[i].getOrigin().getX() - g_pose_b.getOrigin().getX();
+        double y = poses_b[i].getOrigin().getY() - g_pose_b.getOrigin().getY();
+        ptsx_car_b[i] = x * cos(-theta_b) - y * sin(-theta_b);
+        ptsy_car_b[i] = x * sin(-theta_b) + y * cos(-theta_b);
+    }
+    Eigen::VectorXd coeffs_b = mpc_tool.polyfit(ptsx_car_b, ptsy_car_b, 3);
+
+    Eigen::VectorXd state(9);
+    double vx1 = g_cmd_vel_f.linear.x;
+    double vx2 = g_cmd_vel_b.linear.x;
+    double vth1 = g_cmd_vel_f.angular.z;
+    double vth2 = g_cmd_vel_b.angular.z;
+    double dx0 = g_pose_f.getOrigin().getX() - g_pose_b.getOrigin().getX();
+    double dy0 = g_pose_f.getOrigin().getY() - g_pose_b.getOrigin().getY();
+    double dth1 = theta - theta_b;
+    // 距离反向旋转到后车坐标系
+    double dx1 = dx0 * cos(-theta_b) - dy0 * sin(-theta_b);
+    double dy1 = dx0 * sin(-theta_b) + dy0 * cos(-theta_b);
+    // double frontx = g_pose_f.getOrigin().getX();
+    // double fronty = g_pose_f.getOrigin().getY();
+    // double backx = g_pose_b.getOrigin().getX();
+    // double backy = g_pose_b.getOrigin().getY();
+
+    /*printf(" (%.3f %.3f ) , (%.3f %.3f)  (%.3f %.3f | %.3f %.3f)\n",g_pose_f.getOrigin().getX(),
+        g_pose_f.getOrigin().getY(),g_pose_b.getOrigin().getX(),g_pose_b.getOrigin().getY(),
+        g_cmd_vel_f.linear.x,g_cmd_vel_f.angular.z,g_cmd_vel_b.linear.x,g_cmd_vel_b.angular.z);*/
+
+    state << vx1, vx2, vth1, vth2, ref_v, dt, dx1, dy1, dth1;
+    TowBotMPC mpc;
+    std::vector<double> out;
+    if (mpc.Solve(state, g_max_vx, coeffs, coeffs_b, out)) {
+        geometry_msgs::Twist cmd_vel1, cmd_vel2;
+        cmd_vel1.linear.x = out[0];
+        cmd_vel1.linear.y = 0.0;
+        cmd_vel1.angular.z = out[1];
+
+        cmd_vel2.linear.x = out[2];
+        cmd_vel2.linear.y = 0.0;
+        cmd_vel2.angular.z = out[3];
+        publish_cmd(cmd_vel1, cmd_vel2);
+
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(mpc.m_trajectory_f.size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for (unsigned int i = 0; i < mpc.m_trajectory_f.size(); i++) {
+            float x = mpc.m_trajectory_f[i].getOrigin().getX();
+            float y = mpc.m_trajectory_f[i].getOrigin().getY();
+            float rx = x * cos(theta) - y * sin(theta) + g_pose_f.getOrigin().getX();
+            float ry = x * sin(theta) + y * cos(theta) + g_pose_f.getOrigin().getY();
+            gui_path.poses[i].pose.position.x = rx;
+            gui_path.poses[i].pose.position.y = ry;
+            tf::Quaternion quat = mpc.m_trajectory_f[i].getRotation();
+            gui_path.poses[i].pose.orientation.x = quat.x();
+            gui_path.poses[i].pose.orientation.y = quat.y();
+            gui_path.poses[i].pose.orientation.z = quat.z();
+            gui_path.poses[i].pose.orientation.w = quat.w();
+        }
+        //std::cout<<"publish "<<mpc.m_trajectory.size()<<std::endl;
+        g_local_plan_pub.publish(gui_path);
+
+        nav_msgs::Path gui_path_b;
+        gui_path_b.poses.resize(mpc.m_trajectory_b.size());
+        gui_path_b.header.frame_id = "map";
+        gui_path_b.header.stamp = ros::Time::now();
+
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for (unsigned int i = 0; i < mpc.m_trajectory_b.size(); i++) {
+            float x = mpc.m_trajectory_b[i].getOrigin().getX();
+            float y = mpc.m_trajectory_b[i].getOrigin().getY();
+            float rx = x * cos(theta_b) - y * sin(theta_b) + g_pose_b.getOrigin().getX();
+            float ry = x * sin(theta_b) + y * cos(theta_b) + g_pose_b.getOrigin().getY();
+            gui_path_b.poses[i].pose.position.x = rx;
+            gui_path_b.poses[i].pose.position.y = ry;
+            tf::Quaternion quat = mpc.m_trajectory_b[i].getRotation();
+            gui_path_b.poses[i].pose.orientation.x = quat.x();
+            gui_path_b.poses[i].pose.orientation.y = quat.y();
+            gui_path_b.poses[i].pose.orientation.z = quat.z();
+            gui_path_b.poses[i].pose.orientation.w = quat.w();
+        }
+        //std::cout<<"publish "<<mpc.m_trajectory.size()<<std::endl;
+        g_local_plan_pub_b.publish(gui_path_b);
+
+        auto end = std::chrono::system_clock::now();
+        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+        double time =
+            double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
+        //printf(" ref:%.3f, best_v:%.3f, best_th:%.3f, dt:%.3f\n",ref_v,out[0],out[1],time);
+        double sleep_t = (dt - time) * 1000.0;
+        if (sleep_t > 0)
+            usleep(1000 * int(sleep_t));
+
+    }
+}
+
+void thread_MPC()
+{
+    ros::Rate loop_rate(20);
+    while(ros::ok())
+    {
+        if(g_goal_valid==true)
+        {
+            bool ret = MPC();
+            if (ret) {
+
+            }
+            else {
+                ROS_WARN(" MPC failed ... ");
+            }
+        }
+        loop_rate.sleep();
+    }
+
+}
+
+void updateGoal(const geometry_msgs::PoseStamped::ConstPtr& msg)
+{
+    printf("update goal recieve ...  \n");
+    double x=g_pose_f.getOrigin().getX();
+    double y=g_pose_f.getOrigin().getY();
+    double z=g_pose_f.getOrigin().getZ();
+    double qx=g_pose_f.getRotation().getX();
+    double qy=g_pose_f.getRotation().getY();
+    double qz=g_pose_f.getRotation().getZ();
+    double qw=g_pose_f.getRotation().getW();
+    g_goal.setOrigin(tf::Vector3(x+msg->pose.position.x,y,z));
+    tf::Quaternion quat(qx,qy,qz,qw);
+    g_goal.setRotation(quat);
+
+    ///更新轨迹
+    if(maker.Make(g_pose_f,g_goal,10)) {
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(maker.Size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for (unsigned int i = 0; i < maker.Size(); i++) {
+            gui_path.poses[i].pose.position.x = maker[i].getOrigin().getX();
+            gui_path.poses[i].pose.position.y = maker[i].getOrigin().getY();
+            tf::Quaternion quat=maker[i].getRotation();
+
+            gui_path.poses[i].pose.orientation.x = quat.x();
+            gui_path.poses[i].pose.orientation.y = quat.y();
+            gui_path.poses[i].pose.orientation.z = quat.z();
+            gui_path.poses[i].pose.orientation.w = quat.w();
+        }
+
+        g_global_plan_pub.publish(gui_path);
+        g_goal_valid=true;
+    }
+}
+
+int main(int argc ,char* argv[])
+{
+    // std::cout<<"  input max vx : ";
+    // std::cin>>g_max_vx;
+    // std::cout<<std::endl;
+    g_max_vx = 1.0;
+
+    ros::init(argc, argv, "MPC_node");
+
+    ros::NodeHandle gn;
+
+    ros::Subscriber sub = gn.subscribe("/move_base_simple/goal",1,goalCallback);//move_base_simple
+    ros::Subscriber sub0 = gn.subscribe("/goal",1,updateGoal);
+
+    ros::Subscriber pose_sub1_ = gn.subscribe<turtlesim::Pose>( "/turtlesim1/turtle1/pose", 1, poseCallback1);
+    ros::Subscriber pose_sub2_ = gn.subscribe<turtlesim::Pose>( "/turtlesim2/turtle1/pose", 1, poseCallback2);
+
+    g_pose_pub1=gn.advertise<geometry_msgs::PoseStamped>("/turtlePose1", 1);
+    g_pose_pub2=gn.advertise<geometry_msgs::PoseStamped>("/turtlePose2", 1);
+
+    g_vel_pub1 = gn.advertise<geometry_msgs::Twist>("/turtlesim1/turtle1/cmd_vel", 1);
+    g_vel_pub2 = gn.advertise<geometry_msgs::Twist>("/turtlesim2/turtle1/cmd_vel", 1);
+
+    g_global_plan_pub = gn.advertise<nav_msgs::Path>("global_plan",1, true);
+    g_local_plan_pub = gn.advertise<nav_msgs::Path>("local_plan", 1);
+    g_local_plan_pub_b = gn.advertise<nav_msgs::Path>("local_plan_b", 1);
+    g_fit_plan_pub = gn.advertise<nav_msgs::Path>("fit_plan", 1);
+    g_fit_plan_pub_b = gn.advertise<nav_msgs::Path>("fit_plan_b", 1);
+
+    std::thread* pthread=new std::thread(thread_MPC);
+    pthread->detach();
+
+    ros::spin();
+
+
+    geometry_msgs::Twist cmd_vel;
+    cmd_vel.linear.x = 0;
+    cmd_vel.linear.y = 0.0;
+    cmd_vel.angular.z = 0;
+    publish_cmd(cmd_vel,cmd_vel);
+
+    return 0;
+}

src/MPC/src/trajectory/CppAD5Trajectory.cpp

@@ -0,0 +1,244 @@
+//
+// Created by zx on 2019/8/30.
+//
+
+#include "CppAD5Trajectory.h"
+#include <cppad/cppad.hpp>
+#include <cppad/ipopt/solve.hpp>
+#include "Eigen/Core"
+
+
+using CppAD::AD;
+
+class FG5_eval {
+public:
+    double m_x0,m_x1;
+    FG5_eval(double x0,double x1) {
+        m_x0=x0;
+        m_x1=x1;
+    }
+
+    typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
+    void operator()(ADvector& fg, const ADvector& vars) {
+        AD<double> a=vars[0];
+        AD<double> b=vars[1];
+        AD<double> c=vars[2];
+        AD<double> d=vars[3];
+        AD<double> e=vars[4];
+        AD<double> f=vars[5];
+
+        /*AD<double> Fx1=20*a*a*CppAD::pow(m_x1,5)+30*a*b*CppAD::pow(m_x1,4)+(20*a*c+b*b)*CppAD::pow(m_x1,3)
+            +(18*b*c+10*a*d)*CppAD::pow(m_x1,2)+(9*c*c+12*b*d)*m_x1+d*log(m_x1);
+        AD<double> Fx0=20*a*a*CppAD::pow(m_x0,5)+30*a*b*CppAD::pow(m_x0,4)+(20*a*c+b*b)*CppAD::pow(m_x0,3)
+            +(18*b*c+10*a*d)*CppAD::pow(m_x0,2)+(9*c*c+12*b*d)*m_x0+d*CppAD::log(m_x0);*/
+        AD<double> Fx1=100/7.0*a*a*CppAD::pow(m_x1,7)+20*a*b*CppAD::pow(m_x1,6)+(12*a*c+36/5.0*b*b)*CppAD::pow(m_x1,5)
+            +(9*b*c+5*a*d)*CppAD::pow(m_x1,4)+(3*c*c+4*b*d)*CppAD::pow(m_x1,3)+3*c*d*m_x1*m_x1+d*d;
+        AD<double> Fx0=100/7.0*a*a*CppAD::pow(m_x0,7)+20*a*b*CppAD::pow(m_x0,6)+(12*a*c+36/5.0*b*b)*CppAD::pow(m_x0,5)
+            +(9*b*c+5*a*d)*CppAD::pow(m_x0,4)+(3*c*c+4*b*d)*CppAD::pow(m_x0,3)+3*c*d*m_x0*m_x0+d*d;
+        fg[0]=Fx1-Fx0;      //||二阶导数|| * ||x-x1|| 在(xo,x1)上积分
+
+        fg[1]=a*CppAD::pow(m_x0,5)+b*CppAD::pow(m_x0,4)+c*CppAD::pow(m_x0,3)+d*CppAD::pow(m_x0,2)+e*m_x0+f; // ==y0
+        fg[2]=a*CppAD::pow(m_x1,5)+b*CppAD::pow(m_x1,4)+c*CppAD::pow(m_x1,3)+d*CppAD::pow(m_x1,2)+e*m_x1+f; // ==y1
+
+        fg[3]=5*a*CppAD::pow(m_x0,4)+4*b*CppAD::pow(m_x0,3)+3*c*CppAD::pow(m_x0,2)+2*d*m_x0+e ;  // gradient0
+        fg[4]=5*a*CppAD::pow(m_x1,4)+4*b*CppAD::pow(m_x1,3)+3*c*CppAD::pow(m_x1,2)+2*d*m_x1+e ;  // gradient0
+
+        fg[5]=10*a*CppAD::pow(m_x1,3)+6*b*CppAD::pow(m_x1,2)+3*c*m_x1+d ;         //  0
+
+
+
+    }
+};
+
+
+
+CppAD5Trajectory::CppAD5Trajectory(){}
+CppAD5Trajectory::~CppAD5Trajectory(){}
+bool CppAD5Trajectory::Make(tf::Transform t0,tf::Transform t1, int extend_num)
+{
+    position0_=t0;
+    position1_=t1;
+    double angle =gen_axis_angle();
+    tf::Transform pose1=Rotate_pose(position0_,angle);
+    tf::Transform pose2=Rotate_pose(position1_,angle);
+    double gradient1=get_gradient_by_tf(pose1);
+    double gradient2=get_gradient_by_tf(pose2);
+
+    if(Solve(t0,t1)==false) return false;
+
+    std::cout<<"  solve param :"<<m_p;
+    boost::mutex::scoped_lock lock(mutex_trajectory_);
+    double x0=pose1.getOrigin().getX();
+    double y0=pose1.getOrigin().getY();
+    double x1=pose2.getOrigin().getX();
+    double y1=pose2.getOrigin().getY();
+    trajectory_.clear();
+    double a=m_p[0];
+    double b=m_p[1];
+    double c=m_p[2];
+    double d=m_p[3];
+    double e=m_p[4];
+    double f=m_p[5];
+    //计算两轮间距/2
+    double distance=0.5*sqrt(pow(left_tf_.getOrigin().getX()-right_tf_.getOrigin().getX(),2.0)+
+        pow(left_tf_.getOrigin().getY()-right_tf_.getOrigin().getY(),2.0));
+    distance+=0.00000001;
+
+    double delta=0.05;
+    double x=x0;
+    // added by yct
+    int extend_front_count = 0;
+    int extend_end_count = 0;
+    int sign = 1;
+    //modified by yct
+    while(x<x1 || (extend_num>0 && extend_end_count<=extend_num))
+    {
+        double y=a*pow(x,5.0)+b*pow(x,4.0)+c*pow(x,3.0)+d*pow(x,2.0)+e*x+f;
+        double gradient=5.0*a*pow(x,4.0)+4.0*b*pow(x,3.0)+3.0*c*x*x+2.0*d*x+e;
+
+        //计算x处的角度变化率
+        /*double gradtheta=threta_gradient(x);
+        if(fabs(gradtheta)>1.0/distance)
+        {
+            trajectory_.clear();
+            return false;
+        }*/
+
+        tf::Transform pose;
+        pose.setOrigin(tf::Vector3(x,y,0));
+        pose=get_tf(x,y,gradient);
+
+        // modified by yct
+        // 开头逆向移动x到扩展头，再开始入队.
+        if (extend_num > 0)
+        {
+            if(extend_front_count <= extend_num)
+            {
+                extend_front_count++;
+                sign = -1;
+            }else{
+                sign = 1;
+                trajectory_.push_back(pose);
+                if(x>=x1 && extend_end_count <= extend_num){
+                    extend_end_count++;
+                }
+            }
+        }
+        else{
+            trajectory_.push_back(pose);
+        }
+
+        double delta_x=sqrt(delta*delta/(1.0+gradient*gradient));
+        // modified by yct
+        x+=delta * sign;
+    }
+
+    //最后一点加入轨迹中, modified by yct, 非扩展手动加最后一点
+    if (extend_num <= 0)
+    {
+        double x_last = x1;
+        double y_last = a * pow(x_last, 5.0) + b * pow(x_last, 4.0) + c * pow(x_last,3.0)
+            + d*pow(x_last,2.0)+e*x_last+f;
+        double gradient_last = 5.0 * a * pow(x_last, 4.0) + 4.0 * b * pow(x_last,3.0)
+            + 3.0*c*pow(x_last,2.0)+2.0*d*x_last+e;
+        tf::Transform pose_last;
+        pose_last.setOrigin(tf::Vector3(x_last, y_last, 0));
+        pose_last = get_tf(x_last, y_last, gradient_last);
+        trajectory_.push_back(pose_last);
+    }
+
+    //将轨迹点反变换到原坐标系
+    //tf::Transform tf_inv=axis_transform_.inverse();
+    for(int i=0;i<trajectory_.size();++i)
+    {
+        trajectory_[i]=Rotate_pose(trajectory_[i],-angle);//trajectory_[i]*tf_inv;
+    }
+    return true;
+}
+
+bool CppAD5Trajectory::Solve(tf::Pose t0,tf::Pose t1)
+{
+    bool ok=true;
+    position0_=t0;
+    position1_=t1;
+
+    double angle =gen_axis_angle();
+
+    tf::Transform pose1=Rotate_pose(position0_,angle);
+    tf::Transform pose2=Rotate_pose(position1_,angle);
+    double gradient1=get_gradient_by_tf(pose1);
+    double gradient2=get_gradient_by_tf(pose2);
+
+    std::cout<<"grad1 grad2 :"<<gradient1<<" ,"<<gradient2<<std::endl;
+
+    double x0=pose1.getOrigin().getX();
+    double y0=pose1.getOrigin().getY();
+    double x1=pose2.getOrigin().getX();
+    double y1=pose2.getOrigin().getY();
+
+    typedef CPPAD_TESTVECTOR(double) Dvector;
+    size_t n_vars = 6;
+    size_t n_constraints = 5;
+
+    Dvector vars(n_vars);
+    for (int i = 0; i < n_vars; i++) {
+        vars[i] = 0.0;
+    }
+
+    Dvector vars_lowerbound(n_vars);
+    Dvector vars_upperbound(n_vars);
+    for (int i = 0; i < n_vars; i++) {
+        vars_lowerbound[i] = -1.0e19;
+        vars_upperbound[i] = 1.0e19;
+    }
+
+    Dvector constraints_lowerbound(n_constraints);
+    Dvector constraints_upperbound(n_constraints);
+
+    constraints_lowerbound[0] = y0;
+    constraints_upperbound[0] = y0;
+
+    constraints_lowerbound[1] = y1;
+    constraints_upperbound[1] = y1;
+
+    constraints_lowerbound[2] = gradient1;
+    constraints_upperbound[2] = gradient1;
+
+    constraints_lowerbound[3] = gradient2;
+    constraints_upperbound[3] = gradient2;
+
+    constraints_lowerbound[4] = 0;
+    constraints_upperbound[4] = 0;
+
+    // object that computes objective and constraints
+    FG5_eval fg_eval(x0,x1);
+
+    std::string options;
+    options += "Integer print_level  0\n";
+    options += "Sparse  true        forward\n";
+    options += "Sparse  true        reverse\n";
+    options += "Numeric max_cpu_time          0.5\n";
+
+    CppAD::ipopt::solve_result<Dvector> solution;
+    std::cout<<" options :"<<options;
+    CppAD::ipopt::solve<Dvector, FG5_eval>(
+        options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
+        constraints_upperbound, fg_eval, solution);
+
+    // Check some of the solution values
+    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
+    std::cout<<" ok:"<<solution.status<<std::endl;
+    auto cost = solution.obj_value;
+
+    if(ok)
+    {
+        Eigen::VectorXd p(6);
+        p<<solution.x[0],solution.x[1],solution.x[2],solution.x[3],solution.x[4],solution.x[5];
+        std::cout<<" solve p : "<<p;
+        m_p=p;
+    }
+    return ok;
+
+}
+
+

src/MPC/src/trajectory/CppAD5Trajectory.h

@@ -0,0 +1,23 @@
+//
+// Created by zx on 2019/8/30.
+//
+
+#ifndef CPPAD5TRAJECTORY_H
+#define CPPAD5TRAJECTORY_H
+#include "make_trajectory.h"
+
+class CppAD5Trajectory : public MakeTrajectory
+{
+public:
+    CppAD5Trajectory();
+    ~CppAD5Trajectory();
+    virtual bool Make(tf::Transform t0,tf::Transform t1, int extend_num=0);
+protected:
+    bool Solve(tf::Pose pose1,tf::Pose pose2);
+
+protected:
+    Eigen::VectorXd   m_p;
+};
+
+
+#endif //CPPAD4TRAJECTORY_H

src/MPC/src/trajectory/FourthTrajectory.cpp

@@ -0,0 +1,149 @@
+//
+// Created by zx on 2019/8/31.
+//
+
+#include "FourthTrajectory.h"
+
+FourthTrajectory::FourthTrajectory()
+{
+
+}
+FourthTrajectory::~FourthTrajectory()
+{}
+
+bool FourthTrajectory::Make(tf::Transform t0, tf::Transform t1, int extend_num)
+{
+    position0_=t0;
+    position1_=t1;
+
+    double angle =gen_axis_angle();
+
+    tf::Transform pose1=Rotate_pose(position0_,angle);
+    tf::Transform pose2=Rotate_pose(position1_,angle);
+    double gradient1=get_gradient_by_tf(pose1);
+    double gradient2=get_gradient_by_tf(pose2);
+
+    //std::cout<<"grad1 grad2 :"<<gradient1<<" ,"<<gradient2<<std::endl;
+    //三阶多项式差值，生成 曲线
+    double x0=pose1.getOrigin().getX();
+    double y0=pose1.getOrigin().getY();
+    double x1=pose2.getOrigin().getX();
+    double y1=pose2.getOrigin().getY();
+
+    //std::cout<<" x1 ,x2 : "<<x0<<" , "<<x1<<std::endl;
+
+    Eigen::Matrix<double,5,5> A=Eigen::MatrixXd::Zero(5,5);    //系数矩阵A
+    Eigen::Matrix<double,5,1> B=Eigen::MatrixXd::Zero(5,1);    //值矩阵B
+    A(0,0)=pow(x0,4.0);
+    A(0,1)=pow(x0,3.0);
+    A(0,2)=pow(x0,2.0);
+    A(0,3)=x0;
+    A(0,4)=1.0;
+
+    A(1,0)=pow(x1,4.0);
+    A(1,1)=pow(x1,3.0);
+    A(1,2)=pow(x1,2.0);
+    A(1,3)=x1;
+    A(1,4)=1.0;
+
+    A(2,0)=4.0*pow(x0,3.0);
+    A(2,1)=3.0*x0*x0;
+    A(2,2)=2.0*x0;
+    A(2,3)=1.0;
+
+    A(3,0)=4.0*pow(x1,3.0);
+    A(3,1)=3.0*x1*x1;
+    A(3,2)=2.0*x1;
+    A(3,3)=1.0;
+
+    A(4,0)=12*pow(x1,2.0);
+    A(4,1)=6.0*x1;
+    A(4,2)=2.0;
+
+    B(0,0)=y0;
+    B(1,0)=y1;
+    B(2,0)=gradient1;
+    B(3,0)=gradient2;
+    B(4,0)=0;
+    Eigen::MatrixXd P=A.inverse()*B;
+    line_param_=P;
+
+    double a=P(0,0);
+    double b=P(1,0);
+    double c=P(2,0);
+    double d=P(3,0);
+    double e=P(4,0);
+    //生成离散点,间距 step (m)
+
+    boost::mutex::scoped_lock lock(mutex_trajectory_);
+    trajectory_.clear();
+
+    double delta=0.05;
+    double x=x0;
+    // added by yct
+    int extend_front_count = 0;
+    int extend_end_count = 0;
+    int sign = 1;
+    //modified by yct
+    while(x<x1 || (extend_num>0 && extend_end_count<=extend_num))
+    {
+        double y=a*pow(x,4.0)+b*pow(x,3.0)+c*x*x+d*x+e;
+        double gradient=4.0*a*pow(x,3.0)+3.0*b*x*x+2.0*c*x+d;
+
+        //计算x处的角度变化率
+        /*double gradtheta=threta_gradient(x);
+        if(fabs(gradtheta)>1.0/distance)
+        {
+            trajectory_.clear();
+            return false;
+        }*/
+
+        tf::Transform pose;
+        pose.setOrigin(tf::Vector3(x,y,0));
+        pose=get_tf(x,y,gradient);
+
+        // modified by yct
+        // 开头逆向移动x到扩展头，再开始入队.
+        if (extend_num > 0)
+        {
+            if(extend_front_count <= extend_num)
+            {
+                extend_front_count++;
+                sign = -1;
+            }else{
+                sign = 1;
+                trajectory_.push_back(pose);
+                if(x>=x1 && extend_end_count <= extend_num){
+                    extend_end_count++;
+                }
+            }
+        }
+        else{
+            trajectory_.push_back(pose);
+        }
+
+        double delta_x=sqrt(delta*delta/(1.0+gradient*gradient));
+        // modified by yct
+        x+=delta * sign;
+    }
+
+    //最后一点加入轨迹中, modified by yct, 非扩展手动加最后一点
+    if (extend_num <= 0)
+    {
+        double x_last = x1;
+        double y_last = a * pow(x_last, 4.0) + b * pow(x_last, 3.0) + c * x_last*x_last + d*x_last+e;
+        double gradient_last = 4.0 * a * pow(x_last, 3.0) + 3.0 * b * x_last*x_last + 2.0*c*x_last+d;
+        tf::Transform pose_last;
+        pose_last.setOrigin(tf::Vector3(x_last, y_last, 0));
+        pose_last = get_tf(x_last, y_last, gradient_last);
+        trajectory_.push_back(pose_last);
+    }
+
+    //将轨迹点反变换到原坐标系
+    //tf::Transform tf_inv=axis_transform_.inverse();
+    for(int i=0;i<trajectory_.size();++i)
+    {
+        trajectory_[i]=Rotate_pose(trajectory_[i],-angle);//trajectory_[i]*tf_inv;
+    }
+    return true;
+}

src/MPC/src/trajectory/FourthTrajectory.h

@@ -0,0 +1,18 @@
+//
+// Created by zx on 2019/8/31.
+//
+
+#ifndef FOURTHTRAJECTORY_H
+#define FOURTHTRAJECTORY_H
+#include "make_trajectory.h"
+
+class FourthTrajectory :public MakeTrajectory
+{
+public:
+    FourthTrajectory();
+    ~FourthTrajectory();
+    virtual bool Make(tf::Transform t0,tf::Transform t1, int extend_num=0);
+};
+
+
+#endif //FOURTHTRAJECTORY_H

src/MPC/src/trajectory/make_trajectory.cpp

@@ -0,0 +1,282 @@
+#include "make_trajectory.h"
+#include <limits>
+
+double get_pose_angle_z(tf::Transform pos)
+{
+    double yaw,a,b;
+    pos.getBasis().getEulerZYX(yaw,a,b);
+    return yaw;
+}
+
+double get_gradient_by_tf(tf::Transform pos)
+{
+    double angle=get_pose_angle_z(pos);
+    double gradient=tanf(angle);
+    if(fabs(gradient)>200)
+        return 200.0*(gradient/fabs(gradient));
+    return gradient;
+}
+
+tf::Transform get_tf(double x,double y,double gradient)
+{
+    double angle=0;
+    if(fabs(gradient)>200.0)
+    {
+        angle=M_PI/2.0*(gradient/fabs(gradient));
+    }
+    else
+    {
+        angle=atan(fabs(gradient));
+        angle=angle*(gradient/fabs(gradient));
+    }
+    tf::Matrix3x3 rotation;
+    rotation.setRPY(0,0,angle);
+    tf::Transform tf;
+    tf.setBasis(rotation);
+    tf.setOrigin(tf::Vector3(x,y,0));
+    return tf;
+}
+
+tf::Transform transform_tf(tf::Transform pos,tf::Transform tf)
+{
+    pos=pos*tf;
+    return pos;
+}
+
+tf::Pose Rotate_pose(tf::Pose pose,double angle)
+{
+    //先平移，后旋转
+    double x=pose.getOrigin().getX();
+    double y=pose.getOrigin().getY();
+
+    double cs=cos(angle);
+    double sn=sin(angle);
+    double x1=cs*x+sn*y;
+    double y1=-sn*x+cs*y;
+    tf::Pose newpose;
+
+    tf::Quaternion quat;
+    double theta,a,b;
+    pose.getBasis().getEulerZYX(theta,a,b);
+    quat.setEulerZYX(theta-angle,a,b);
+    newpose.setRotation(quat);
+    newpose.setOrigin(tf::Vector3(x1,y1,0));
+    return newpose;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+MakeTrajectory::MakeTrajectory()
+{
+    left_tf_=tf::Transform(tf::Quaternion(0,0,0));
+    right_tf_=tf::Transform(tf::Quaternion(0,0,0));
+    
+}
+bool MakeTrajectory::TransformTrajectory(tf::Transform tf)
+{
+
+    return true;
+}
+tf::Transform MakeTrajectory::operator[](int n)
+{
+    boost::mutex::scoped_lock lock(mutex_trajectory_);
+    return trajectory_[n];
+
+}
+bool MakeTrajectory::Make(tf::Transform t0,tf::Transform t1, int extend_num)
+{
+    position0_=t0;
+    position1_=t1;
+
+    double angle =gen_axis_angle();
+
+    tf::Transform pose1=Rotate_pose(position0_,angle);
+    tf::Transform pose2=Rotate_pose(position1_,angle);
+    double gradient1=get_gradient_by_tf(pose1);
+    double gradient2=get_gradient_by_tf(pose2);
+    
+    //std::cout<<"grad1 grad2 :"<<gradient1<<" ,"<<gradient2<<std::endl;
+    //三阶多项式差值，生成 曲线
+    double x0=pose1.getOrigin().getX();
+    double y0=pose1.getOrigin().getY();
+    double x1=pose2.getOrigin().getX();
+    double y1=pose2.getOrigin().getY();
+
+    //std::cout<<" x1 ,x2 : "<<x0<<" , "<<x1<<std::endl;
+
+    Eigen::Matrix<double,4,4> A=Eigen::MatrixXd::Zero(4,4);    //系数矩阵A
+    Eigen::Matrix<double,4,1> B=Eigen::MatrixXd::Zero(4,1);    //值矩阵B
+    A(0,0)=pow(x0,3.0);
+    A(0,1)=pow(x0,2.0);
+    A(0,2)=x0;
+    A(0,3)=1.0;
+
+    A(1,0)=pow(x1,3.0);
+    A(1,1)=pow(x1,2.0);
+    A(1,2)=x1;
+    A(1,3)=1.0;
+
+    A(2,0)=3.0*pow(x0,2.0);
+    A(2,1)=2.0*x0;
+    A(2,2)=1.0;
+    
+    A(3,0)=3.0*pow(x1,2.0);
+    A(3,1)=2.0*x1;
+    A(3,2)=1.0;
+
+    B(0,0)=y0;
+    B(1,0)=y1;
+    B(2,0)=gradient1;
+    B(3,0)=gradient2;
+    Eigen::MatrixXd P=A.inverse()*B;
+    line_param_=P;
+
+    double a=P(0,0);
+    double b=P(1,0);
+    double c=P(2,0);
+    double d=P(3,0);
+    //生成离散点,间距 step (m)
+
+    boost::mutex::scoped_lock lock(mutex_trajectory_);
+    trajectory_.clear();
+
+    //计算两轮间距/2
+    double distance=0.5*sqrt(pow(left_tf_.getOrigin().getX()-right_tf_.getOrigin().getX(),2.0)+
+                            pow(left_tf_.getOrigin().getY()-right_tf_.getOrigin().getY(),2.0));
+    distance+=0.00000001;
+
+    double delta=0.05;
+    double x=x0;
+    // added by yct
+    int extend_front_count = 0;
+    int extend_end_count = 0;
+    int sign = 1;
+    //modified by yct
+    while(x<x1 || (extend_num>0 && extend_end_count<=extend_num))
+    {
+        double y=a*pow(x,3.0)+b*pow(x,2.0)+c*x+d;
+        double gradient=3.0*a*pow(x,2.0)+2.0*b*x+c;
+
+        //计算x处的角度变化率
+        double gradtheta=threta_gradient(x);
+        if(fabs(gradtheta)>1.0/distance)
+        {
+            trajectory_.clear();
+            return false;
+        }
+
+        tf::Transform pose;
+        pose.setOrigin(tf::Vector3(x,y,0));
+        pose=get_tf(x,y,gradient);
+
+        // modified by yct
+        // 开头逆向移动x到扩展头，再开始入队.
+        if (extend_num > 0)
+        {
+            if(extend_front_count <= extend_num)
+            {
+                extend_front_count++;
+                sign = -1;
+            }else{
+                sign = 1;
+                trajectory_.push_back(pose);
+                if(x>=x1 && extend_end_count <= extend_num){
+                    extend_end_count++;
+                }
+            }
+        }
+        else{
+            trajectory_.push_back(pose);
+        }
+
+        double delta_x=sqrt(delta*delta/(1.0+gradient*gradient));
+        // modified by yct
+        x+=delta * sign;
+    }
+
+    //最后一点加入轨迹中, modified by yct, 非扩展手动加最后一点
+    if (extend_num <= 0)
+    {
+        double x_last = x1;
+        double y_last = a * pow(x_last, 3.0) + b * pow(x_last, 2.0) + c * x_last + d;
+        double gradient_last = 3.0 * a * pow(x_last, 2.0) + 2.0 * b * x_last + c;
+        tf::Transform pose_last;
+        pose_last.setOrigin(tf::Vector3(x_last, y_last, 0));
+        pose_last = get_tf(x_last, y_last, gradient_last);
+        trajectory_.push_back(pose_last);
+    }
+
+    //将轨迹点反变换到原坐标系
+    //tf::Transform tf_inv=axis_transform_.inverse();
+    for(int i=0;i<trajectory_.size();++i)
+    {
+        trajectory_[i]=Rotate_pose(trajectory_[i],-angle);//trajectory_[i]*tf_inv;
+    }
+    return true;
+}
+double MakeTrajectory::gen_axis_angle()
+{
+
+    double x=position1_.getOrigin().getX()-position0_.getOrigin().getX();
+    double y=position1_.getOrigin().getY()-position0_.getOrigin().getY();
+    tf::Vector3 var(x,y,0);
+
+    double angle=atan2(y,x);
+
+    //std::cout<<" var.angle : "<<angle*180.0/M_PI<<std::endl;
+    return angle;
+
+}
+
+void MakeTrajectory::SetLRTransform(tf::Transform left,tf::Transform right)
+{
+    left_tf_=left;
+    right_tf_=right;
+}
+
+bool MakeTrajectory::IsPoseInTrajectory(tf::Transform pose)
+{
+    boost::mutex::scoped_lock lock(mutex_trajectory_);
+    if(trajectory_.size()==0)
+        return false;
+
+    double angle0=get_pose_angle_z(pose);
+    double min_distance=std::numeric_limits<double>::max();
+    tf::Transform min_point;
+    for(int i=0;i<trajectory_.size();++i)
+    {
+        tf::Transform tf=trajectory_[i];
+        double distance=sqrt(pow(pose.getOrigin().getX()-tf.getOrigin().getX(),2.0)+
+                            pow(pose.getOrigin().getY()-tf.getOrigin().getY(),2.0));
+        if(distance < min_distance)
+        {
+            min_distance=distance;
+            min_point=tf;
+        }
+    }
+    if(min_distance==std::numeric_limits<double>::max())
+        return false;
+    double angle1=get_pose_angle_z(min_point);
+    double angle_distance=fabs(angle1-angle0);
+    
+    if(min_distance>0.05 || angle_distance*180.0/M_PI>10.0)
+    {
+        std::cout<<"distance :"<<min_distance<<"   angle distance : "<<angle_distance*180.0/M_PI<<std::endl;
+        return false;
+    }
+
+    return true;
+}
+
+double MakeTrajectory::threta_gradient(double x)
+{
+    double a=line_param_(0,0);
+    double b=line_param_(1,0);
+    double c=line_param_(2,0);
+    double d=line_param_(3,0);
+    double S=1+pow(3*a*pow(x,2.0)+2*b*x+c,2.0);
+    double s=6*a*x+2*b;
+    return s/S;
+}

src/MPC/src/trajectory/make_trajectory.h

@@ -0,0 +1,47 @@
+#ifndef __GEN_TRAJECTORY__
+#define __GEN_TRAJECTORY__
+/*
+ * 三次曲线获取轨迹，两点坐标+两点梯度 求曲线参数
+ */
+#include <tf/tf.h>
+#include "tf2_msgs/TFMessage.h"
+#include <boost/thread.hpp>
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <vector>
+
+double get_pose_angle_z(tf::Transform pos);
+double get_gradient_by_tf(tf::Transform pos);
+tf::Transform get_tf(double x,double y,double gradient);
+tf::Transform transform_tf(tf::Transform pos,tf::Transform tf);
+tf::Pose Rotate_pose(tf::Pose pose,double angle);
+
+
+class MakeTrajectory
+{
+public:
+    MakeTrajectory();
+    bool TransformTrajectory(tf::Transform tf);
+    void SetLRTransform(tf::Transform letf,tf::Transform right);
+    tf::Transform operator[](int n);
+    int Size(){return trajectory_.size();}
+    virtual bool Make(tf::Transform t0,tf::Transform t1, int extend_num=0);
+    bool IsPoseInTrajectory(tf::Transform pose);
+protected:
+    //计算当前x点的 角度变化率
+    double threta_gradient(double x);
+    double  gen_axis_angle();
+public:
+    std::vector<tf::Transform>      trajectory_;                //雷达轨迹
+protected:
+    tf::Transform                   position0_;
+    tf::Transform                   position1_;
+
+    tf::Transform                   left_tf_;
+    tf::Transform                   right_tf_;
+    Eigen::MatrixXd                 line_param_;                //曲线参数
+
+    boost::mutex	                mutex_trajectory_;
+};
+
+#endif

src/agv_bundle/CMakeLists.txt

@@ -0,0 +1,190 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(agv_bundle)
+
+#set(OpenCV_DIR "/opt/ros/kinetic/share/OpenCV-3.3.1-dev/")
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
+set(CMAKE_BUILD_TYPE "RELEASE")
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  sensor_msgs
+  tf
+)
+find_package(OpenCV REQUIRED)
+find_package(Ceres REQUIRED)
+find_package(PCL REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES ceres_test
+#  CATKIN_DEPENDS roscpp rospy
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+  ${CERES_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+  common/
+  error_code/
+  locate/
+  moving_control/
+  odom/
+  /usr/local/include/cifx
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/ceres_test.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+aux_source_directory(src/locate LOCATE)
+aux_source_directory(src/common COMMON)
+aux_source_directory(src/error_code ERROR_CODE)
+aux_source_directory(src/moving_control MOVING_CONTROL)
+aux_source_directory(src/odom ODOM)
+
+include_directories(
+${LOCATE}
+${MOVING_CONTROL}
+${ERROR_CODE}
+${COMMON}
+${ODOM}
+)
+
+add_executable(${PROJECT_NAME}_locate ${LOCATE} src/ceres_scan_match_node.cpp)
+target_link_libraries(${PROJECT_NAME}_locate
+  ${catkin_LIBRARIES}
+  ${CERES_LIBRARIES}
+  ${OpenCV_LIBS}
+  ${PCL_LIBRARIES}
+)
+
+add_executable(${PROJECT_NAME}_move_ctrl ${MOVING_CONTROL} src/moving_control_node.cpp)
+target_link_libraries(${PROJECT_NAME}_move_ctrl
+  ${catkin_LIBRARIES}
+  ipopt
+)
+
+# if(CIFX_HEADER OR CIFX_INC_LIB)
+#    set(LIBCIFX_INCLUDE_DIRS  ${CIFX_HEADER})
+#    set(LIBCIFX_LIBRARIES "-pthread -lrt -lcifx -L${CIFX_INC_LIB}")
+# else(CIFX_HEADER OR CIFX_INC_LIB)
+#    include(FindPkgConfig)
+#    pkg_check_modules(LIBCIFX REQUIRED cifx)
+# endif(CIFX_HEADER OR CIFX_INC_LIB)
+
+# add_executable(${PROJECT_NAME}_odom ${ODOM} ${COMMON} ${ERROR_CODE} src/odom_node.cpp)
+# target_link_libraries(${PROJECT_NAME}_odom
+#  ${catkin_LIBRARIES}
+#  ${LIBCIFX_LIBRARIES}
+# )

src/agv_bundle/configuration_files/200521map_origin.yaml

@@ -0,0 +1,6 @@
+image: 200521map_origin.pgm
+resolution: 0.010000
+origin: [-19.701677, -9.709916, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

src/agv_bundle/configuration_files/200622map_origin.yaml

@@ -0,0 +1,6 @@
+image: /home/zx/zzw/agv_ws/src/agv_bundle/configuration_files/200622map_origin.pgm
+resolution: 0.010000
+origin: [-7.488005, -10.328027, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

src/agv_bundle/configuration_files/200702map_origin.yaml

@@ -0,0 +1,6 @@
+image: /home/zx/zzw/agv_ws/src/agv_bundle/configuration_files/200702map_origin.pgm
+resolution: 0.010000
+origin: [-13.350000, -10.982070, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

src/agv_bundle/configuration_files/blank_map.yaml

@@ -0,0 +1,6 @@
+image: blank_map.pgm
+resolution: 0.02
+origin: [-2.5, -2.5, 0]
+occupied_thresh: 0.65
+free_thresh: 0.196 # Taken from the Willow Garage map in the turtlebot_navigation package
+negate: 0

src/agv_bundle/launch/MPC.launch

@@ -0,0 +1,10 @@
+<launch>
+  <group ns="turtlesim1">
+     <node pkg="turtlesim" name="sim" type="turtlesim_node"/>
+   </group>
+
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find agv_bundle)/configuration_files/blank_map.yaml"/>
+  <node pkg="agv_bundle" type="agv_bundle_move_ctrl" name="agv_bundle_move_ctrl" output="screen"/>
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find agv_bundle)/launch/default.rviz" />
+
+</launch>

src/agv_bundle/launch/locate.sh

@@ -0,0 +1,24 @@
+
+time=$(date "+%Y-%m-%d-%H-%M-%S")
+
+# gnome-terminal -x bash -c "roscore"
+
+
+gnome-terminal -x bash -c "cd ~/zzw/agv_ws; \
+source devel/setup.bash; \
+rosrun agv_bundle agv_bundle_locate"
+# > ~/zzw/agv_ws/src/agv_bundle/logs/${time}-locate.log"
+
+gnome-terminal -x bash -c "cd ~/zzw/agv_ws; \
+source devel/setup.bash; \
+roslaunch agv_bundle map_scan.launch"
+# > ~/zzw/agv_ws/src/agv_bundle/logs/map_server.log"
+
+# gnome-terminal -x bash -c "cd ~/Documents/git_ws; \
+# source devel/setup.bash; \
+# rosrun agv_bundle agv_bundle_move_ctrl > ~/Documents/git_ws/src/agv_bundle/logs/${time}-move_ctrl.log"
+
+#gnome-terminal -x bash -c "cd ~/Documents/git_ws; \
+#source devel/setup.bash; \
+#roslaunch agv_bundle rosbag.launch > ~/Documents/git_ws/src/agv_bundle/logs/rosbag.log"
+#roslaunch agv_bundle rosbag.launch > ~/Documents/git_ws/src/agv_bundle/logs/rosbag.log

src/agv_bundle/launch/map_scan.launch

@@ -0,0 +1,6 @@
+<launch>
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find agv_bundle)/configuration_files/200702map_origin.yaml"/>
+  <node name="rviz" pkg="rviz" type="rviz" required="true"
+      args="-d $(find agv_bundle)/configuration_files/demo.rviz" />
+  <!--node name="agv_bundle" pkg="agv_bundle" type="agv_bundle_locate"/-->
+</launch>

src/agv_bundle/launch/rosbag.launch

@@ -0,0 +1,9 @@
+<launch>
+  <arg name="file" default="$(find agv_bundle)/launch/gedian_fast1.bag"/>
+  <node pkg="rosbag" type="play" name="playback" ns="bag_replay" args="
+	--clock $(arg file) -r 1 --topics /scan "
+	output="screen">
+    <remap from="/scan_urg" to="/scan"/>
+
+  </node>
+</launch>

src/agv_bundle/launch/user_uio.sh

@@ -0,0 +1,4 @@
+sudo modprobe uio_netx
+sudo chmod 666 /dev/uio0
+sudo chmod 666 /sys/class/uio/uio0/device/config
+

src/agv_bundle/package.xml

@@ -0,0 +1,69 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>agv_bundle</name>
+  <version>0.0.0</version>
+  <description>The agv_bundle package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="youchen@todo.todo">youchen</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/map_scan_match</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>tf</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/agv_bundle/scan.txt

@@ -0,0 +1,846 @@
+-0.0722 -0.0735 0.0026
+-0.0695 -0.0719 0.0000
+-0.0705 -0.0737 0.0000
+-0.0764 -0.0805 0.0000
+-0.0713 -0.0758 0.0000
+-0.0702 -0.0753 0.0000
+-0.0706 -0.0764 0.0000
+-0.0703 -0.0767 0.0000
+-0.0686 -0.0755 0.0000
+-0.0679 -0.0761 0.0000
+-0.0682 -0.0771 0.0000
+-0.0686 -0.0782 0.0000
+-0.0669 -0.0770 0.0000
+-0.0666 -0.0773 0.0000
+-0.0455 -0.0902 0.0000
+-0.0464 -0.0931 0.0000
+-0.0509 -0.1031 0.0000
+-0.0530 -0.1088 0.0000
+-0.0517 -0.1072 0.0000
+-0.0495 -0.1038 0.0000
+-0.0452 -0.0959 0.0000
+-0.0486 -0.1042 0.0000
+-0.0440 -0.0954 0.0000
+-0.0456 -0.1001 0.0000
+-0.0452 -0.1003 0.0000
+-0.0456 -0.1023 0.0000
+-0.0403 -0.0915 0.0000
+-0.0415 -0.0954 0.0000
+-0.0482 -0.1121 0.0000
+-0.0481 -0.1132 0.0000
+-0.0476 -0.1134 0.0000
+-0.0467 -0.1127 0.0000
+-0.0462 -0.1129 0.0000
+-0.0450 -0.1113 0.0000
+-0.0489 -0.1226 0.0000
+-0.0480 -0.1219 0.0000
+-0.0431 -0.1109 0.0000
+-0.0498 -0.1298 0.0000
+-0.0475 -0.1253 0.0000
+-0.0448 -0.1199 0.0000
+-0.0478 -0.1295 0.0000
+-1.0496 -3.0484 0.0000
+-1.0363 -3.0529 0.0000
+-1.1402 -5.2516 0.0000
+-1.3730 -6.4597 0.0000
+-1.3448 -6.4656 0.0000
+-1.3138 -6.4577 0.0000
+-1.2488 -6.2780 0.0000
+-1.2061 -6.2049 0.0000
+-1.1790 -6.2101 0.0000
+-1.3492 -9.0277 0.0000
+-1.3167 -9.0810 0.0000
+-1.2704 -9.0392 0.0000
+-1.3449 -9.8819 0.0000
+-1.2993 -9.8688 0.0000
+-0.3897 -9.9183 0.0000
+-0.3466 -9.9260 0.0000
+-0.3033 -9.9274 0.0000
+-0.2599 -9.9246 0.0000
+-0.2166 -9.9286 0.0000
+-0.1733 -9.9305 0.0000
+-0.1302 -9.9471 0.0000
+-0.0868 -9.9506 0.0000
+-0.0434 -9.9409 0.0000
+0.0000 -9.9390 0.0000
+0.0433 -9.9319 0.0000
+0.0868 -9.9436 0.0000
+0.1300 -9.9341 0.0000
+0.1732 -9.9215 0.0000
+0.2165 -9.9206 0.0000
+0.2598 -9.9226 0.0000
+0.3022 -9.8924 0.0000
+0.3460 -9.9070 0.0000
+0.3885 -9.8874 0.0000
+0.4316 -9.8856 0.0000
+0.3762 -7.8330 0.0000
+0.4077 -7.7793 0.0000
+0.4418 -7.7805 0.0000
+0.4754 -7.7725 0.0000
+0.6504 -5.9385 0.0000
+0.6661 -5.8462 0.0000
+0.6771 -5.7211 0.0000
+0.6802 -5.5394 0.0000
+0.6912 -5.4332 0.0000
+0.5262 -3.9965 0.0000
+0.5410 -3.9754 0.0000
+0.5584 -3.9730 0.0000
+0.5731 -3.9527 0.0000
+0.5918 -3.9600 0.0000
+0.6103 -3.9653 0.0000
+0.6351 -4.0100 0.0000
+0.7166 -4.4000 0.0000
+0.7315 -4.3712 0.0000
+0.7355 -4.2803 0.0000
+0.7392 -4.1923 0.0000
+0.7401 -4.0926 0.0000
+0.7442 -4.0156 0.0000
+0.7472 -3.9357 0.0000
+0.7539 -3.8784 0.0000
+0.7540 -3.7907 0.0000
+0.7532 -3.7022 0.0000
+0.7573 -3.6411 0.0000
+0.7622 -3.5859 0.0000
+0.7660 -3.5278 0.0000
+0.7662 -3.4561 0.0000
+0.7694 -3.4000 0.0000
+0.7723 -3.3450 0.0000
+0.7745 -3.2890 0.0000
+0.7732 -3.2205 0.0000
+0.7760 -3.1714 0.0000
+0.7804 -3.1302 0.0000
+0.7798 -3.0705 0.0000
+0.7822 -3.0245 0.0000
+0.7824 -2.9717 0.0000
+0.7832 -2.9229 0.0000
+0.7830 -2.8722 0.0000
+0.7897 -2.8475 0.0000
+0.7888 -2.7969 0.0000
+0.7905 -2.7569 0.0000
+0.7936 -2.7227 0.0000
+0.7944 -2.6818 0.0000
+0.7948 -2.6410 0.0000
+0.7967 -2.6059 0.0000
+0.7950 -2.5604 0.0000
+0.7969 -2.5273 0.0000
+0.7981 -2.4934 0.0000
+0.7997 -2.4613 0.0000
+0.8023 -2.4331 0.0000
+0.7980 -2.3850 0.0000
+0.8039 -2.3683 0.0000
+0.8081 -2.3468 0.0000
+0.8058 -2.3074 0.0000
+0.8078 -2.2812 0.0000
+0.8124 -2.2626 0.0000
+0.8085 -2.2214 0.0000
+0.8106 -2.1972 0.0000
+0.8065 -2.1572 0.0000
+0.7429 -1.9610 0.0000
+0.6691 -1.7430 0.0000
+0.6701 -1.7233 0.0000
+0.6711 -1.7036 0.0000
+0.6755 -1.6932 0.0000
+0.6844 -1.6940 0.0000
+0.6884 -1.6826 0.0000
+0.6980 -1.6852 0.0000
+0.6957 -1.6590 0.0000
+0.7029 -1.6560 0.0000
+0.7228 -1.6823 0.0000
+0.7309 -1.6810 0.0000
+0.7398 -1.6814 0.0000
+0.7464 -1.6764 0.0000
+0.7611 -1.6895 0.0000
+0.7854 -1.7235 0.0000
+0.8202 -1.7791 0.0000
+0.8275 -1.7746 0.0000
+0.8288 -1.7574 0.0000
+0.8283 -1.7366 0.0000
+0.8267 -1.7140 0.0000
+0.8268 -1.6951 0.0000
+0.8284 -1.6798 0.0000
+0.8308 -1.6664 0.0000
+0.8318 -1.6502 0.0000
+0.8276 -1.6243 0.0000
+0.8301 -1.6118 0.0000
+0.8339 -1.6019 0.0000
+0.8353 -1.5877 0.0000
+0.8436 -1.5867 0.0000
+0.8515 -1.5847 0.0000
+0.8613 -1.5863 0.0000
+0.8716 -1.5886 0.0000
+0.8824 -1.5918 0.0000
+0.8917 -1.5923 0.0000
+0.8987 -1.5884 0.0000
+0.9071 -1.5871 0.0000
+0.9155 -1.5857 0.0000
+0.9285 -1.5920 0.0000
+0.9364 -1.5897 0.0000
+0.9459 -1.5899 0.0000
+0.9528 -1.5858 0.0000
+0.9670 -1.5936 0.0000
+0.9755 -1.5919 0.0000
+0.9845 -1.5910 0.0000
+0.9984 -1.5977 0.0000
+1.0080 -1.5976 0.0000
+1.0176 -1.5974 0.0000
+1.0279 -1.5980 0.0000
+1.0370 -1.5968 0.0000
+1.0472 -1.5973 0.0000
+1.0581 -1.5986 0.0000
+1.0684 -1.5989 0.0000
+1.0826 -1.6050 0.0000
+1.0896 -1.6003 0.0000
+1.1028 -1.6046 0.0000
+1.1138 -1.6055 0.0000
+1.1213 -1.6014 0.0000
+1.1301 -1.5990 0.0000
+1.1393 -1.5973 0.0000
+1.1498 -1.5972 0.0000
+1.1673 -1.6067 0.0000
+1.1785 -1.6072 0.0000
+1.1873 -1.6045 0.0000
+1.2014 -1.6089 0.0000
+1.2115 -1.6077 0.0000
+1.2239 -1.6095 0.0000
+1.2346 -1.6089 0.0000
+1.2446 -1.6075 0.0000
+1.2590 -1.6115 0.0000
+1.2636 -1.6028 0.0000
+1.2793 -1.6083 0.0000
+1.2907 -1.6081 0.0000
+1.2995 -1.6048 0.0000
+1.3103 -1.6038 0.0000
+1.3129 -1.5926 0.0000
+1.3038 -1.5677 0.0000
+1.3061 -1.5566 0.0000
+1.3065 -1.5433 0.0000
+1.3093 -1.5330 0.0000
+1.3075 -1.5174 0.0000
+1.3088 -1.5056 0.0000
+1.3088 -1.4924 0.0000
+1.3087 -1.4792 0.0000
+1.3098 -1.4675 0.0000
+1.3108 -1.4558 0.0000
+1.3111 -1.4434 0.0000
+1.3120 -1.4318 0.0000
+1.3087 -1.4158 0.0000
+1.3081 -1.4027 0.0000
+1.3142 -1.3970 0.0000
+1.3127 -1.3833 0.0000
+1.3097 -1.3682 0.0000
+1.3143 -1.3610 0.0000
+1.3244 -1.3595 0.0000
+1.3443 -1.3680 0.0000
+1.3580 -1.3699 0.0000
+1.3647 -1.3647 0.0000
+1.3756 -1.3637 0.0000
+1.3901 -1.3661 0.0000
+1.3968 -1.3607 0.0000
+1.4121 -1.3636 0.0000
+1.4245 -1.3637 0.0000
+1.4370 -1.3636 0.0000
+1.4487 -1.3628 0.0000
+1.4664 -1.3674 0.0000
+1.4753 -1.3637 0.0000
+1.4908 -1.3660 0.0000
+1.5108 -1.3723 0.0000
+1.5249 -1.3731 0.0000
+1.5309 -1.3664 0.0000
+1.5428 -1.3650 0.0000
+1.5556 -1.3642 0.0000
+1.5706 -1.3653 0.0000
+1.5826 -1.3636 0.0000
+1.6029 -1.3690 0.0000
+1.6165 -1.3685 0.0000
+1.6317 -1.3691 0.0000
+1.6484 -1.3710 0.0000
+1.6667 -1.3739 0.0000
+1.6936 -1.3837 0.0000
+1.7035 -1.3795 0.0000
+1.7228 -1.3827 0.0000
+1.7405 -1.3845 0.0000
+1.7536 -1.3824 0.0000
+1.7707 -1.3834 0.0000
+1.7877 -1.3842 0.0000
+1.8065 -1.3861 0.0000
+1.8252 -1.3879 0.0000
+1.8440 -1.3896 0.0000
+1.8589 -1.3881 0.0000
+1.8786 -1.3901 0.0000
+1.8968 -1.3908 0.0000
+1.9109 -1.3883 0.0000
+1.9389 -1.3958 0.0000
+1.9441 -1.3867 0.0000
+1.9518 -1.3794 0.0000
+1.9660 -1.3766 0.0000
+1.9810 -1.3743 0.0000
+2.0158 -1.3854 0.0000
+2.0276 -1.3806 0.0000
+2.0626 -1.3913 0.0000
+2.0770 -1.3878 0.0000
+2.1014 -1.3909 0.0000
+2.1200 -1.3899 0.0000
+2.1428 -1.3916 0.0000
+2.1589 -1.3887 0.0000
+2.1903 -1.3954 0.0000
+2.2293 -1.4066 0.0000
+2.2762 -1.4223 0.0000
+2.3342 -1.4445 0.0000
+2.4079 -1.4755 0.0000
+2.7776 -1.6855 0.0000
+2.8184 -1.6934 0.0000
+2.8421 -1.6908 0.0000
+2.8744 -1.6931 0.0000
+2.9154 -1.7002 0.0000
+2.9505 -1.7035 0.0000
+2.9797 -1.7030 0.0000
+3.0062 -1.7008 0.0000
+3.0450 -1.7053 0.0000
+3.0769 -1.7056 0.0000
+3.1150 -1.7090 0.0000
+3.1444 -1.7073 0.0000
+3.1747 -1.7058 0.0000
+3.2201 -1.7122 0.0000
+3.2656 -1.7181 0.0000
+3.2908 -1.7131 0.0000
+3.3045 -1.7019 0.0000
+3.3137 -1.6884 0.0000
+3.3183 -1.6726 0.0000
+3.3220 -1.6563 0.0000
+3.3202 -1.6374 0.0000
+3.3139 -1.6163 0.0000
+3.3200 -1.6014 0.0000
+3.3233 -1.5851 0.0000
+3.3266 -1.5689 0.0000
+3.3280 -1.5519 0.0000
+3.3229 -1.5319 0.0000
+3.3286 -1.5169 0.0000
+3.3225 -1.4967 0.0000
+3.3217 -1.4789 0.0000
+3.3235 -1.4624 0.0000
+3.3188 -1.4431 0.0000
+3.3205 -1.4266 0.0000
+3.3221 -1.4101 0.0000
+3.3227 -1.3933 0.0000
+3.3269 -1.3780 0.0000
+3.3273 -1.3612 0.0000
+3.3184 -1.3407 0.0000
+3.3205 -1.3247 0.0000
+3.3290 -1.3113 0.0000
+3.3291 -1.2946 0.0000
+3.3273 -1.2772 0.0000
+3.3263 -1.2602 0.0000
+3.3252 -1.2432 0.0000
+3.3278 -1.2277 0.0000
+3.3322 -1.2128 0.0000
+3.3280 -1.1949 0.0000
+3.3285 -1.1787 0.0000
+3.3345 -1.1645 0.0000
+3.3226 -1.1440 0.0000
+3.3294 -1.1302 0.0000
+3.3343 -1.1156 0.0000
+3.3420 -1.1020 0.0000
+3.3468 -1.0874 0.0000
+3.3410 -1.0695 0.0000
+3.3390 -1.0528 0.0000
+3.3340 -1.0352 0.0000
+3.3299 -1.0180 0.0000
+3.3343 -1.0035 0.0000
+3.3386 -0.9889 0.0000
+3.3391 -0.9732 0.0000
+3.3413 -0.9581 0.0000
+3.3426 -0.9427 0.0000
+3.3428 -0.9271 0.0000
+3.3401 -0.9106 0.0000
+3.3431 -0.8958 0.0000
+3.3460 -0.8809 0.0000
+3.3488 -0.8661 0.0000
+3.3400 -0.8482 0.0000
+3.3456 -0.8341 0.0000
+3.3511 -0.8200 0.0000
+3.3411 -0.8021 0.0000
+3.3348 -0.7852 0.0000
+3.3392 -0.7709 0.0000
+3.3493 -0.7579 0.0000
+3.3536 -0.7435 0.0000
+3.3519 -0.7278 0.0000
+3.3550 -0.7131 0.0000
+3.3434 -0.6954 0.0000
+3.3396 -0.6794 0.0000
+3.3396 -0.6643 0.0000
+3.3385 -0.6489 0.0000
+3.3393 -0.6340 0.0000
+3.3470 -0.6203 0.0000
+3.3418 -0.6043 0.0000
+3.3464 -0.5901 0.0000
+3.3529 -0.5761 0.0000
+3.3573 -0.5618 0.0000
+3.3558 -0.5465 0.0000
+3.3532 -0.5311 0.0000
+3.3525 -0.5160 0.0000
+3.3557 -0.5015 0.0000
+3.3460 -0.4851 0.0000
+3.3610 -0.4724 0.0000
+3.3699 -0.4586 0.0000
+3.3481 -0.4408 0.0000
+3.3659 -0.4282 0.0000
+3.3677 -0.4135 0.0000
+3.3695 -0.3988 0.0000
+3.3712 -0.3841 0.0000
+3.3738 -0.3695 0.0000
+3.3744 -0.3547 0.0000
+3.3710 -0.3394 0.0000
+3.3704 -0.3245 0.0000
+3.3668 -0.3094 0.0000
+3.3761 -0.2954 0.0000
+3.3724 -0.2802 0.0000
+3.3706 -0.2653 0.0000
+3.3767 -0.2509 0.0000
+3.3778 -0.2362 0.0000
+3.3788 -0.2215 0.0000
+3.3767 -0.2065 0.0000
+3.3766 -0.1917 0.0000
+3.3774 -0.1770 0.0000
+3.3781 -0.1623 0.0000
+3.3768 -0.1474 0.0000
+3.3854 -0.1330 0.0000
+3.3919 -0.1184 0.0000
+3.3874 -0.1035 0.0000
+3.3808 -0.0885 0.0000
+3.3782 -0.0737 0.0000
+3.3785 -0.0590 0.0000
+3.3827 -0.0443 0.0000
+3.3769 -0.0295 0.0000
+3.3850 -0.0148 0.0000
+3.3870 0.0000 0.0000
+3.3910 0.0148 0.0000
+3.3929 0.0296 0.0000
+3.3877 0.0443 0.0000
+3.3875 0.0591 0.0000
+3.3852 0.0739 0.0000
+3.3808 0.0885 0.0000
+3.3684 0.1029 0.0000
+3.1851 0.1112 0.0000
+2.9267 0.1150 0.0000
+2.8843 0.1259 0.0000
+2.8557 0.1372 0.0000
+2.8361 0.1486 0.0000
+2.8304 0.1607 0.0000
+2.8137 0.1721 0.0000
+2.8090 0.1841 0.0000
+2.8121 0.1966 0.0000
+2.8093 0.2088 0.0000
+2.8083 0.2210 0.0000
+2.8113 0.2336 0.0000
+2.8152 0.2463 0.0000
+2.8122 0.2584 0.0000
+2.8080 0.2704 0.0000
+2.8008 0.2820 0.0000
+2.7946 0.2937 0.0000
+2.7933 0.3059 0.0000
+2.7899 0.3179 0.0000
+2.8024 0.3317 0.0000
+2.8000 0.3438 0.0000
+2.7975 0.3559 0.0000
+2.7969 0.3682 0.0000
+2.8022 0.3814 0.0000
+2.8054 0.3943 0.0000
+2.8037 0.4065 0.0000
+2.7880 0.4167 0.0000
+2.7872 0.4290 0.0000
+2.7873 0.4415 0.0000
+2.7873 0.4539 0.0000
+2.7892 0.4668 0.0000
+2.7872 0.4789 0.0000
+2.7909 0.4921 0.0000
+2.8006 0.5064 0.0000
+2.8013 0.5192 0.0000
+2.7990 0.5314 0.0000
+2.7967 0.5436 0.0000
+2.7943 0.5558 0.0000
+2.8006 0.5698 0.0000
+2.8069 0.5838 0.0000
+2.8034 0.5959 0.0000
+2.8076 0.6096 0.0000
+2.8127 0.6236 0.0000
+2.8129 0.6365 0.0000
+2.8140 0.6497 0.0000
+2.8316 0.6667 0.0000
+2.8325 0.6800 0.0000
+2.8742 0.7033 0.0000
+2.9177 0.7275 0.0000
+2.9833 0.7577 0.0000
+3.0129 0.7792 0.0000
+2.7832 0.7327 0.0000
+2.6457 0.7089 0.0000
+2.6049 0.7102 0.0000
+2.5748 0.7141 0.0000
+2.5467 0.7182 0.0000
+2.5618 0.7346 0.0000
+2.5729 0.7499 0.0000
+2.5821 0.7649 0.0000
+2.5740 0.7747 0.0000
+2.5686 0.7853 0.0000
+2.5633 0.7959 0.0000
+2.5722 0.8110 0.0000
+2.5724 0.8234 0.0000
+2.5717 0.8356 0.0000
+2.5756 0.8493 0.0000
+2.5756 0.8618 0.0000
+2.5775 0.8750 0.0000
+2.5737 0.8862 0.0000
+2.5641 0.8954 0.0000
+2.5593 0.9063 0.0000
+2.5694 0.9225 0.0000
+2.5757 0.9375 0.0000
+2.5716 0.9487 0.0000
+2.3349 0.8963 0.0000
+2.3310 0.9065 0.0000
+2.3112 0.9104 0.0000
+2.0963 0.8363 0.0000
+2.0500 0.8283 0.0000
+2.0186 0.8258 0.0000
+1.9984 0.8277 0.0000
+1.9707 0.8264 0.0000
+1.9515 0.8283 0.0000
+1.9285 0.8286 0.0000
+1.9057 0.8286 0.0000
+1.8883 0.8309 0.0000
+1.8746 0.8346 0.0000
+1.8618 0.8387 0.0000
+1.8399 0.8385 0.0000
+1.8190 0.8386 0.0000
+1.8054 0.8419 0.0000
+1.7836 0.8412 0.0000
+1.7664 0.8425 0.0000
+1.7510 0.8446 0.0000
+1.7410 0.8491 0.0000
+1.7301 0.8532 0.0000
+1.7040 0.8496 0.0000
+1.6886 0.8511 0.0000
+1.6689 0.8503 0.0000
+1.6571 0.8535 0.0000
+1.6463 0.8570 0.0000
+1.6328 0.8591 0.0000
+1.6123 0.8573 0.0000
+1.6050 0.8624 0.0000
+1.6003 0.8689 0.0000
+1.6079 0.8821 0.0000
+1.6093 0.8920 0.0000
+1.6063 0.8996 0.0000
+1.6058 0.9085 0.0000
+1.6044 0.9170 0.0000
+1.6108 0.9300 0.0000
+1.6085 0.9380 0.0000
+1.6044 0.9450 0.0000
+1.6037 0.9541 0.0000
+1.6106 0.9678 0.0000
+1.6081 0.9758 0.0000
+1.6106 0.9870 0.0000
+1.6114 0.9972 0.0000
+1.6164 1.0100 0.0000
+1.6153 1.0192 0.0000
+1.6100 1.0257 0.0000
+1.6140 1.0381 0.0000
+1.6203 1.0522 0.0000
+1.6190 1.0615 0.0000
+1.6186 1.0713 0.0000
+1.6180 1.0811 0.0000
+1.6216 1.0938 0.0000
+1.6209 1.1037 0.0000
+1.6186 1.1124 0.0000
+1.6039 1.1126 0.0000
+1.4671 1.0273 0.0000
+1.2478 0.8819 0.0000
+1.1845 0.8449 0.0000
+1.1573 0.8331 0.0000
+1.1391 0.8276 0.0000
+1.1419 0.8373 0.0000
+1.1479 0.8494 0.0000
+1.1498 0.8586 0.0000
+1.1492 0.8660 0.0000
+1.1550 0.8783 0.0000
+1.1543 0.8857 0.0000
+1.1631 0.9006 0.0000
+1.1639 0.9093 0.0000
+1.1623 0.9163 0.0000
+1.1669 0.9282 0.0000
+1.1644 0.9345 0.0000
+1.1618 0.9408 0.0000
+1.1631 0.9503 0.0000
+1.1628 0.9586 0.0000
+1.1610 0.9656 0.0000
+1.1606 0.9738 0.0000
+1.1586 0.9808 0.0000
+1.1665 0.9963 0.0000
+1.1659 1.0046 0.0000
+1.1660 1.0136 0.0000
+1.1714 1.0273 0.0000
+1.1744 1.0390 0.0000
+1.1743 1.0481 0.0000
+1.1742 1.0572 0.0000
+1.1666 1.0596 0.0000
+1.1619 1.0647 0.0000
+1.1690 1.0807 0.0000
+1.1716 1.0926 0.0000
+1.1705 1.1011 0.0000
+1.1729 1.1131 0.0000
+1.1717 1.1216 0.0000
+1.1747 1.1344 0.0000
+1.1697 1.1395 0.0000
+1.1676 1.1474 0.0000
+1.1746 1.1644 0.0000
+1.1717 1.1717 0.0000
+1.1729 1.1832 0.0000
+1.1747 1.1954 0.0000
+1.1786 1.2098 0.0000
+1.1740 1.2157 0.0000
+1.1811 1.2338 0.0000
+1.1778 1.2411 0.0000
+1.1744 1.2484 0.0000
+1.1730 1.2579 0.0000
+1.1798 1.2763 0.0000
+1.1796 1.2873 0.0000
+1.1787 1.2976 0.0000
+1.1770 1.3072 0.0000
+1.1773 1.3190 0.0000
+1.1801 1.3339 0.0000
+1.1769 1.3420 0.0000
+1.1829 1.3607 0.0000
+1.1815 1.3712 0.0000
+1.1820 1.3839 0.0000
+1.1850 1.3998 0.0000
+1.1859 1.4134 0.0000
+1.1862 1.4262 0.0000
+1.1876 1.4406 0.0000
+1.1870 1.4528 0.0000
+1.1831 1.4610 0.0000
+1.1811 1.4716 0.0000
+1.1778 1.4807 0.0000
+1.1893 1.5086 0.0000
+1.1821 1.5130 0.0000
+1.1797 1.5237 0.0000
+1.2145 1.5827 0.0000
+1.2669 1.6660 0.0000
+1.2656 1.6795 0.0000
+1.2553 1.6810 0.0000
+1.2432 1.6801 0.0000
+1.2347 1.6839 0.0000
+1.2255 1.6868 0.0000
+1.2094 1.6800 0.0000
+1.1974 1.6787 0.0000
+1.1878 1.6806 0.0000
+1.1804 1.6858 0.0000
+1.1691 1.6852 0.0000
+1.1577 1.6845 0.0000
+1.1498 1.6887 0.0000
+1.1419 1.6929 0.0000
+1.1300 1.6912 0.0000
+1.1138 1.6828 0.0000
+1.1048 1.6851 0.0000
+1.0964 1.6882 0.0000
+1.0890 1.6930 0.0000
+1.0725 1.6834 0.0000
+1.0683 1.6931 0.0000
+1.0609 1.6978 0.0000
+1.0398 1.6803 0.0000
+1.0325 1.6848 0.0000
+1.0215 1.6833 0.0000
+1.0131 1.6860 0.0000
+1.0057 1.6905 0.0000
+0.9958 1.6905 0.0000
+0.9859 1.6905 0.0000
+0.9795 1.6965 0.0000
+0.9761 1.7077 0.0000
+0.9720 1.7181 0.0000
+0.9719 1.7354 0.0000
+0.9813 1.7702 0.0000
+0.9769 1.7806 0.0000
+0.9662 1.7796 0.0000
+2.3745 4.6104 0.0000
+2.3548 4.6217 0.0000
+2.3032 4.5694 0.0000
+2.2747 4.5624 0.0000
+2.2548 4.5723 0.0000
+2.1801 4.5707 0.0000
+2.1601 4.5802 0.0000
+2.1435 4.5968 0.0000
+2.1184 4.5952 0.0000
+2.0938 4.5944 0.0000
+2.0762 4.6090 0.0000
+2.0544 4.6143 0.0000
+2.0262 4.6049 0.0000
+1.9914 4.5798 0.0000
+1.9710 4.5875 0.0000
+1.9505 4.5952 0.0000
+1.0298 4.6452 0.0000
+1.0106 4.6546 0.0000
+0.9946 4.6795 0.0000
+0.9671 4.6495 0.0000
+0.9486 4.6625 0.0000
+0.9265 4.6577 0.0000
+0.8957 4.6078 0.0000
+0.8755 4.6116 0.0000
+0.8550 4.6134 0.0000
+0.8474 4.6860 0.0000
+1.1499 6.5214 0.0000
+1.1182 6.5076 0.0000
+1.0893 6.5095 0.0000
+1.0612 6.5161 0.0000
+1.0323 6.5178 0.0000
+1.0034 6.5192 0.0000
+0.9745 6.5206 0.0000
+0.9456 6.5218 0.0000
+0.9172 6.5259 0.0000
+0.8881 6.5258 0.0000
+0.8585 6.5207 0.0000
+0.8294 6.5195 0.0000
+0.8003 6.5181 0.0000
+0.7722 6.5245 0.0000
+0.7436 6.5268 0.0000
+0.7144 6.5230 0.0000
+0.6854 6.5211 0.0000
+0.6568 6.5230 0.0000
+0.6284 6.5258 0.0000
+0.6001 6.5305 0.0000
+0.5717 6.5350 0.0000
+0.5498 6.6172 0.0000
+0.6340 8.0551 0.0000
+0.5988 8.0578 0.0000
+0.5055 7.2293 0.0000
+-0.6364 4.6759 0.0000
+-0.6587 4.6869 0.0000
+-0.6771 4.6702 0.0000
+-0.6974 4.6662 0.0000
+-0.7167 4.6562 0.0000
+-0.7370 4.6530 0.0000
+-0.7590 4.6606 0.0000
+-0.7845 4.6878 0.0000
+-0.8059 4.6903 0.0000
+-0.8240 4.6729 0.0000
+-0.8443 4.6693 0.0000
+-0.8662 4.6734 0.0000
+-0.8864 4.6686 0.0000
+-0.9067 4.6647 0.0000
+-0.7552 3.7966 0.0000
+-0.7638 3.7541 0.0000
+-0.7763 3.7321 0.0000
+-1.7400 8.1861 0.0000
+-1.7766 8.1824 0.0000
+-0.7982 3.6006 0.0000
+-1.6724 7.3911 0.0000
+-1.6912 7.3253 0.0000
+-0.7969 3.3844 0.0000
+-0.7722 2.9858 0.0000
+-0.7852 2.9824 0.0000
+-0.7842 2.9268 0.0000
+-0.7970 2.9233 0.0000
+-0.8119 2.9275 0.0000
+-1.8754 6.6496 0.0000
+-1.9091 6.6577 0.0000
+-0.8331 2.8581 0.0000
+-0.8268 2.7911 0.0000
+-2.0180 6.7049 0.0000
+-2.0486 6.7008 0.0000
+-2.0678 6.6594 0.0000
+-0.8246 2.5006 0.0000
+-0.8355 2.4969 0.0000
+-0.8549 2.2864 0.0000
+-0.8592 2.2677 0.0000
+-0.8655 2.2546 0.0000
+-0.0460 0.1184 0.0000
+-0.0440 0.1117 0.0000
+-0.0422 0.1059 0.0000
+-0.0476 0.1178 0.0000
+-0.0488 0.1194 0.0000
+-0.0486 0.1173 0.0000
+-0.0437 0.1042 0.0000
+-0.0461 0.1086 0.0000
+-0.0446 0.1038 0.0000
+-0.0475 0.1091 0.0000
+-0.0471 0.1071 0.0000
+-0.0476 0.1069 0.0000
+-0.0460 0.1021 0.0000
+-0.0431 0.0946 0.0000
+-0.0431 0.0935 0.0000
+-0.0423 0.0906 0.0000
+-0.0431 0.0913 0.0000
+-0.0452 0.0948 0.0000
+-0.0461 0.0955 0.0000
+-0.0447 0.0917 0.0000
+-0.0447 0.0906 0.0000
+-0.0513 0.1029 0.0000
+-0.0554 0.1098 0.0000
+-0.0513 0.1007 0.0000
+-0.0513 0.0996 0.0000
+-0.0531 0.1020 0.0000
+-0.0559 0.1062 0.0000
+-0.0582 0.1095 0.0000
+-0.0559 0.1039 0.0000
+-0.0577 0.1063 0.0000
+-0.0529 0.0964 0.0000
+-0.0596 0.1076 0.0000
+-0.0562 0.1003 0.0000
+-0.0606 0.1071 0.0000
+-0.0615 0.1077 0.0000
+-0.0560 0.0970 0.0000
+-0.0549 0.0942 0.0000
+-0.0589 0.0999 0.0000
+-0.0634 0.1066 0.0000
+-0.0670 0.1114 0.0000
+-0.0607 0.1000 0.0000
+-0.0690 0.1125 0.0000
+-0.2189 0.3537 0.0000
+-0.2321 0.3714 0.0000
+-0.2481 0.3933 0.0000
+-0.0623 0.0978 0.0000
+-0.9051 1.4071 0.0000
+-0.9112 1.4031 0.0000
+-0.9124 1.3916 0.0000
+-0.9112 1.3767 0.0000
+-0.8956 1.3403 0.0000
+-0.8640 1.2809 0.0000
+-0.8515 1.2506 0.0000
+-0.8490 1.2354 0.0000
+-0.8544 1.2316 0.0000
+-0.8581 1.2255 0.0000
+-0.8588 1.2152 0.0000
+-0.8577 1.2024 0.0000
+-0.8571 1.1906 0.0000
+-0.8552 1.1771 0.0000
+-0.8609 1.1742 0.0000
+-0.8619 1.1648 0.0000
+-0.8598 1.1514 0.0000
+-0.8546 1.1341 0.0000
+-0.8619 1.1335 0.0000
+-0.8638 1.1258 0.0000
+-0.8724 1.1267 0.0000
+-0.8810 1.1276 0.0000
+-0.9026 1.1450 0.0000
+-0.9195 1.1559 0.0000
+-0.9239 1.1511 0.0000
+-0.9415 1.1626 0.0000
+-0.9478 1.1600 0.0000
+-0.9548 1.1582 0.0000
+-0.9656 1.1610 0.0000
+-0.9745 1.1613 0.0000
+-0.9828 1.1609 0.0000
+-0.9891 1.1581 0.0000
+-0.9981 1.1583 0.0000
+-1.0057 1.1570 0.0000
+-1.0121 1.1541 0.0000
+-1.0218 1.1549 0.0000
+-1.0308 1.1549 0.0000
+-1.0412 1.1563 0.0000
+-1.0516 1.1577 0.0000
+-1.0559 1.1524 0.0000
+-1.0705 1.1580 0.0000
+-1.0755 1.1533 0.0000
+-0.3159 0.3358 0.0000
+-0.2547 0.2684 0.0000
+-0.2178 0.2275 0.0000
+-0.2133 0.2208 0.0000
+-0.2233 0.2292 0.0000
+-0.2551 0.2596 0.0000
+-0.2901 0.2926 0.0000
+-0.3493 0.3493 0.0000

src/agv_bundle/scan_line.txt

@@ -0,0 +1,170 @@
+2.62779999 0.72880000 0.00000000
+2.63330007 0.74269998 0.00000000
+2.65499997 0.76130003 0.00000000
+2.65840006 0.77480000 0.00000000
+2.65019989 0.78500003 0.00000000
+2.64100003 0.79479998 0.00000000
+2.63940001 0.80690002 0.00000000
+2.62820005 0.81610000 0.00000000
+2.61800003 0.82539999 0.00000000
+2.60770011 0.83469999 0.00000000
+2.59540009 0.84329998 0.00000000
+2.59080005 0.85430002 0.00000000
+2.56240010 0.85740000 0.00000000
+2.55859995 0.86849999 0.00000000
+2.54629993 0.87680000 0.00000000
+2.54239988 0.88789999 0.00000000
+2.53850007 0.89889997 0.00000000
+2.53080010 0.90869999 0.00000000
+2.52589989 0.91930002 0.00000000
+2.50869989 0.92549998 0.00000000
+2.49900007 0.93440002 0.00000000
+2.48370004 0.94099998 0.00000000
+2.47589993 0.95039999 0.00000000
+2.47449994 0.96230000 0.00000000
+2.46650004 0.97160000 0.00000000
+2.45479989 0.97939998 0.00000000
+2.44959998 0.98970002 0.00000000
+2.43790007 0.99739999 0.00000000
+2.42980003 1.00650001 0.00000000
+2.41989994 1.01470006 0.00000000
+2.41630006 1.02569997 0.00000000
+2.41000009 1.03540003 0.00000000
+2.40089989 1.04390001 0.00000000
+2.39080000 1.05200005 0.00000000
+2.37980008 1.05949998 0.00000000
+2.37150002 1.06830001 0.00000000
+2.36129999 1.07609999 0.00000000
+2.35479999 1.08560002 0.00000000
+2.34010005 1.09119999 0.00000000
+2.32990003 1.09879994 0.00000000
+2.31780005 1.10560000 0.00000000
+2.31660008 1.11740005 0.00000000
+2.30990005 1.12660003 0.00000000
+2.29329991 1.13090003 0.00000000
+2.28830004 1.14090002 0.00000000
+2.27259994 1.14549994 0.00000000
+2.27920008 1.16129994 0.00000000
+2.27049994 1.16939998 0.00000000
+2.26719999 1.18019998 0.00000000
+2.24880004 1.18309999 0.00000000
+2.24620008 1.19430006 0.00000000
+2.22340012 1.19470000 0.00000000
+2.22160006 1.20630002 0.00000000
+2.21900010 1.21739995 0.00000000
+2.21190000 1.22609997 0.00000000
+2.20479989 1.23469996 0.00000000
+2.19589996 1.24240005 0.00000000
+2.17659998 1.24399996 0.00000000
+2.16510010 1.25000000 0.00000000
+2.16129994 1.26040006 0.00000000
+2.15319991 1.26830006 0.00000000
+2.14510012 1.27620006 0.00000000
+2.14120007 1.28659999 0.00000000
+2.13560009 1.29589999 0.00000000
+2.11540008 1.29630005 0.00000000
+2.10630012 1.30340004 0.00000000
+2.10400009 1.31470001 0.00000000
+2.09990001 1.32500005 0.00000000
+2.09330010 1.33360004 0.00000000
+2.08909988 1.34379995 0.00000000
+2.08489990 1.35399997 0.00000000
+2.07229996 1.35870004 0.00000000
+2.06299996 1.36549997 0.00000000
+2.05290008 1.37170005 0.00000000
+2.03940010 1.37559998 0.00000000
+2.03010011 1.38220000 0.00000000
+2.02239990 1.38999999 0.00000000
+2.01220012 1.39590001 0.00000000
+2.00119996 1.40120006 0.00000000
+1.99829996 1.41229999 0.00000000
+1.99539995 1.42330003 0.00000000
+1.98189998 1.42670000 0.00000000
+1.98780000 1.44420004 0.00000000
+1.98140001 1.45280004 0.00000000
+1.97749996 1.46329999 0.00000000
+1.96229994 1.46529996 0.00000000
+1.93509996 1.45819998 0.00000000
+1.92949998 1.46720004 0.00000000
+1.90960002 1.46529996 0.00000000
+1.90160000 1.47239995 0.00000000
+1.90149999 1.48559999 0.00000000
+1.89579999 1.49450004 0.00000000
+1.88999999 1.50339997 0.00000000
+1.88030005 1.50909996 0.00000000
+1.87210000 1.51600003 0.00000000
+1.86399996 1.52289999 0.00000000
+1.85810006 1.53170002 0.00000000
+1.85140002 1.53980005 0.00000000
+1.84459996 1.54779994 0.00000000
+1.82949996 1.54879999 0.00000000
+1.81359994 1.54890001 0.00000000
+1.82500005 1.57249999 0.00000000
+1.81280005 1.57589996 0.00000000
+1.81340003 1.59029996 0.00000000
+1.80799997 1.59959996 0.00000000
+1.80019999 1.60679996 0.00000000
+1.79770005 1.61860001 0.00000000
+1.77499998 1.61230004 0.00000000
+1.76650000 1.61870003 0.00000000
+1.75800002 1.62500000 0.00000000
+1.75090003 1.63269997 0.00000000
+1.75320005 1.64919996 0.00000000
+1.74960005 1.66030002 0.00000000
+1.73800004 1.66380000 0.00000000
+1.72640002 1.66719997 0.00000000
+1.72409999 1.67960000 0.00000000
+1.71679997 1.68710005 0.00000000
+1.70940006 1.69459999 0.00000000
+1.69990003 1.69990003 0.00000000
+1.69459999 1.70940006 0.00000000
+1.68710005 1.71679997 0.00000000
+1.67960000 1.72409999 0.00000000
+1.67200005 1.73150003 0.00000000
+1.66310000 1.73730004 0.00000000
+1.65890002 1.74820006 0.00000000
+1.65540004 1.75969994 0.00000000
+1.64909995 1.76839995 0.00000000
+1.63460004 1.76820004 0.00000000
+1.62609994 1.77460003 0.00000000
+1.62170005 1.78540003 0.00000000
+1.61600006 1.79470003 0.00000000
+1.60810006 1.80170000 0.00000000
+1.59759998 1.80569994 0.00000000
+1.59430003 1.81789994 0.00000000
+1.58500004 1.82340002 0.00000000
+1.57449996 1.82720006 0.00000000
+1.56519997 1.83260000 0.00000000
+1.55850005 1.84089994 0.00000000
+1.55170000 1.84920001 0.00000000
+1.54359996 1.85599995 0.00000000
+1.53359997 1.86039996 0.00000000
+1.52479994 1.86629999 0.00000000
+1.51789999 1.87450004 0.00000000
+1.51040006 1.88189995 0.00000000
+1.51020002 1.89859998 0.00000000
+1.50189996 1.90520000 0.00000000
+1.49300003 1.91090000 0.00000000
+1.48590004 1.91900003 0.00000000
+1.47749996 1.92550004 0.00000000
+1.46840000 1.93110001 0.00000000
+1.45940006 1.93669999 0.00000000
+1.45330000 1.94620001 0.00000000
+1.44780004 1.95659995 0.00000000
+1.43920004 1.96290004 0.00000000
+1.42949998 1.96749997 0.00000000
+1.42209995 1.97539997 0.00000000
+1.41340005 1.98160005 0.00000000
+1.40769994 1.99179995 0.00000000
+1.40470004 2.00609994 0.00000000
+1.39250004 2.00729990 0.00000000
+1.38150001 2.00999999 0.00000000
+1.37549996 2.02020001 0.00000000
+1.36829996 2.02870011 0.00000000
+1.36059999 2.03629994 0.00000000
+1.35109997 2.04139996 0.00000000
+1.34220004 2.04719996 0.00000000
+1.33329999 2.05310011 0.00000000
+1.32700002 2.06310010 0.00000000
+1.30620003 2.05029988 0.00000000
+1.30420005 2.06699991 0.00000000

src/agv_bundle/src/ceres_scan_match_node.cpp

@@ -0,0 +1,28 @@
+#include "locate/ceres_scan_match.h"
+#include<ceres/ceres.h>
+
+using namespace std;
+using namespace cv;
+
+//主函数
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "ceres_scan_matcher");
+  Eigen::Vector3d origin_trans(0,0,0);
+  if(argc >= 4){
+    origin_trans << stod(argv[1]), stod(argv[2]), stod(argv[3]);
+  }
+  Ceres_scan_match sm("/home/zx/zzw/agv_ws/src/agv_bundle/configuration_files/200702map_origin.yaml", origin_trans);
+  // sm.Solve();
+
+  ros::Rate loop_rate(100);
+  while(ros::ok()){
+
+    // sm.Solve();
+    // std::cout << trans << std::endl;
+    ros::spinOnce();
+    loop_rate.sleep();
+  }
+  return 0;
+}
+

src/agv_bundle/src/common/dtype.h

@@ -0,0 +1,40 @@
+//
+// Created by zx on 2020/4/28.
+//
+
+#ifndef AGV_DTYPE_H
+#define AGV_DTYPE_H
+namespace agv
+{
+/**
+ * 位姿数据类型
+ */
+    class Pose
+    {
+    public:
+        Pose();
+        Pose(double x,double y,double theta);
+        double x();
+        double y();
+        double theta();
+
+    protected:
+        double m_x;
+        double m_y;
+        double m_theta;
+    };
+
+
+    /*
+     * agv各项驱动数据
+     */
+#define MOTOR_NUM   2
+    typedef struct AGV_DRIVER_DATE
+    {
+        double front_motor_data[MOTOR_NUM]; //前车电机数据
+        double rear_motor_data[MOTOR_NUM];  //后车电机数据
+    }Driver_data;
+
+}
+
+#endif //AGV_DTYPE_H

src/agv_bundle/src/error_code/error_code.cpp

@@ -0,0 +1,526 @@
+
+//Error_code是错误码的底层通用模块，
+//功能：用作故障分析和处理。
+
+//用法：所有的功能接口函数return错误管理类，
+//然后上层判断分析错误码，并进行故障处理。
+
+
+
+#include "error_code.h"
+
+/////////////////////////////////////////////
+//构造函数
+Error_manager::Error_manager()
+{
+    m_error_code = SUCCESS;
+    m_error_level = NORMAL;
+    pm_error_description = 0;
+    m_description_length = 0;
+    return ;
+}
+//拷贝构造
+Error_manager::Error_manager(const Error_manager & error_manager)
+{
+    this->m_error_code = error_manager.m_error_code;
+    this->m_error_level = error_manager.m_error_level;
+    pm_error_description = NULL;
+    m_description_length = 0;
+    reallocate_memory_and_copy_string(error_manager.pm_error_description, error_manager.m_description_length);
+    return ;
+}
+//赋值构造
+Error_manager::Error_manager(Error_code error_code, Error_level error_level,
+    const char* p_error_description, int description_length)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    pm_error_description = NULL;
+    m_description_length = 0;
+    reallocate_memory_and_copy_string(p_error_description, description_length);
+    return ;
+}
+//赋值构造
+Error_manager::Error_manager(Error_code error_code, Error_level error_level , std::string & error_aggregate_string)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    pm_error_description = NULL;
+    m_description_length = 0;
+    reallocate_memory_and_copy_string(error_aggregate_string);
+    return ;
+}
+//析构函数
+Error_manager::~Error_manager()
+{
+    free_description();
+}
+
+//初始化
+void Error_manager::error_manager_init()
+{
+    error_manager_clear_all();
+    return;
+}
+//初始化
+void Error_manager::error_manager_init(Error_code error_code, Error_level error_level, const char* p_error_description, int description_length)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    reallocate_memory_and_copy_string(p_error_description, description_length);
+    return ;
+}
+//初始化
+void Error_manager::error_manager_init(Error_code error_code, Error_level error_level , std::string & error_aggregate_string)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    reallocate_memory_and_copy_string(error_aggregate_string);
+    return ;
+}
+//重置
+void Error_manager::error_manager_reset(Error_code error_code, Error_level error_level, const char* p_error_description, int description_length)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    reallocate_memory_and_copy_string(p_error_description, description_length);
+    return ;
+}
+//重置
+void Error_manager::error_manager_reset(Error_code error_code, Error_level error_level , std::string & error_aggregate_string)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    reallocate_memory_and_copy_string(error_aggregate_string);
+    return ;
+}
+//重置
+void Error_manager::error_manager_reset(const Error_manager & error_manager)
+{
+    this->m_error_code = error_manager.m_error_code;
+    this->m_error_level = error_manager.m_error_level;
+    reallocate_memory_and_copy_string(error_manager.pm_error_description, error_manager.m_description_length);
+    return ;
+}
+//清除所有内容
+void Error_manager::error_manager_clear_all()
+{
+    m_error_code = SUCCESS;
+    m_error_level = NORMAL;
+    free_description();
+}
+
+//重载=
+Error_manager& Error_manager::operator=(const Error_manager & error_manager)
+{
+    error_manager_reset(error_manager);
+}
+//重载=，支持Error_manager和Error_code的直接转化，会清空错误等级和描述
+Error_manager& Error_manager::operator=(Error_code error_code)
+{
+    error_manager_clear_all();
+    set_error_code(error_code);
+}
+//重载==
+bool Error_manager::operator==(const Error_manager & error_manager)
+{
+    is_equal_error_manager(error_manager);
+}
+//重载==，支持Error_manager和Error_code的直接比较
+bool Error_manager::operator==(Error_code error_code)
+{
+    if(m_error_code == error_code)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+//重载!=
+bool Error_manager::operator!=(const Error_manager & error_manager)
+{
+    ! is_equal_error_manager(error_manager);
+}
+//重载!=，支持Error_manager和Error_code的直接比较
+bool Error_manager::operator!=(Error_code error_code)
+{
+    if(m_error_code != error_code)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+//获取错误码
+Error_code Error_manager::get_error_code()
+{
+    return m_error_code;
+}
+//获取错误等级
+Error_level Error_manager::get_error_level()
+{
+    return m_error_level;
+}
+//获取错误描述的指针，（浅拷贝）
+char* Error_manager::get_error_description()
+{
+    return pm_error_description;
+}
+
+//复制错误描述，（深拷贝）
+//output：p_error_description     错误描述的字符串指针，不可以为NULL，必须要有实际的内存
+//output:description_length      错误描述的字符串长度，不可以为0，长度最好足够大，一般256即可。
+void Error_manager::copy_error_description(const char* p_error_description, int description_length)
+{
+    if(p_error_description != NULL && pm_error_description != NULL)
+    {
+        char *pt_source = (char *)p_error_description;
+        char* pt_destination = pm_error_description;
+
+        int t_length_min = m_description_length;
+        if(m_description_length > description_length)
+        {
+            t_length_min = description_length;
+        }
+
+        for(int i=0;i<t_length_min; i++)
+        {
+            *pt_destination = *pt_source;
+            pt_destination++;
+            pt_source++;
+        }
+    }
+
+    return;
+}
+//复制错误描述，（深拷贝）
+//output：error_description_string     错误描述的string
+void Error_manager::copy_error_description(std::string & error_description_string)
+{
+    if( (!error_description_string.empty() ) && pm_error_description != NULL)
+    {
+        error_description_string = pm_error_description;
+    }
+    return;
+}
+
+//设置错误码
+void Error_manager::set_error_code(Error_code error_code)
+{
+    m_error_code = error_code;
+    return;
+}
+//比较错误等级并升级，取高等级的结果
+void Error_manager::set_error_level_up(Error_level error_level)
+{
+    if(m_error_level < error_level)
+    {
+        m_error_level = error_level;
+    }
+    return;
+}
+//比较错误等级并降级，取低等级的结果
+void Error_manager::set_error_level_down(Error_level error_level)
+{
+    if(m_error_level > error_level)
+    {
+        m_error_level = error_level;
+    }
+    return;
+}
+//错误等级，设定到固定值
+void Error_manager::set_error_level_location(Error_level error_level)
+{
+    m_error_level = error_level;
+    return;
+}
+//设置错误描述
+void Error_manager::set_error_description(const char* p_error_description, int description_length)
+{
+    reallocate_memory_and_copy_string(p_error_description, description_length);
+    return ;
+}
+//设置错误描述
+void Error_manager::set_error_description(std::string & error_description_string)
+{
+    reallocate_memory_and_copy_string(error_description_string);
+    return ;
+}
+
+//尾部追加错误描述
+void Error_manager::add_error_description(const char* p_error_description, int description_length)
+{
+    if(p_error_description !=NULL)
+    {
+        char* pt_description_front = pm_error_description;
+        int t_description_front_length = m_description_length;
+        char* pt_description_back = (char *)p_error_description;
+        int t_description_back_length = 0;
+
+        if(description_length == 0)
+        {
+            t_description_back_length = 0;
+            while (*pt_description_back != '\0')
+            {
+                t_description_back_length++;
+                pt_description_back++;
+            }
+            t_description_back_length++;
+            pt_description_back = (char *)p_error_description;
+        }
+        else
+        {
+            t_description_back_length = description_length;
+        }
+
+        int t_description_new_length = t_description_front_length + 5 + t_description_back_length - 1;
+        char* pt_description_new =  (char*) malloc(t_description_new_length );
+
+        sprintf(pt_description_new, "%s ### %s", pt_description_front, pt_description_back);
+        free_description();
+        pm_error_description = pt_description_new;
+        m_description_length = t_description_new_length;
+    }
+    return ;
+}
+//尾部追加错误描述
+void Error_manager::add_error_description(std::string & error_description_string)
+{
+    if( ! error_description_string.empty() )
+    {
+        std::string t_description_string = pm_error_description ;
+        t_description_string += (" ### "+ error_description_string);
+
+        reallocate_memory_and_copy_string(t_description_string);
+    }
+}
+
+//比较错误是否相同，
+// 注：只比较错误码和等级
+bool Error_manager::is_equal_error_manager(const Error_manager & error_manager)
+{
+    if(this->m_error_code == error_manager.m_error_code
+       && this->m_error_level == error_manager.m_error_level)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+//比较并覆盖错误，讲低级错误转为字符串存放于描述中，
+//如果错误相同，则保留this的，将输入参数转入描述。
+void Error_manager::compare_and_cover_error(const Error_manager & error_manager)
+{
+    if(this->m_error_code == SUCCESS)
+    {
+        error_manager_reset(error_manager);
+    }
+    else if (error_manager.m_error_code == SUCCESS)
+    {
+		return;
+    }
+    else
+    {
+        Error_manager t_error_manager_new;
+        char* pt_string_inside = NULL;
+        int t_string_inside_length = 0;
+        if(this->m_error_level < error_manager.m_error_level)
+        {
+            t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + this->m_description_length;
+            pt_string_inside = (char*)malloc(t_string_inside_length);
+            translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+            error_manager_reset(error_manager);
+            add_error_description(pt_string_inside,t_string_inside_length);
+        }
+        else
+        {
+            t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + error_manager.m_description_length;
+            pt_string_inside = (char*)malloc(t_string_inside_length);
+			((Error_manager & )error_manager).translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+            add_error_description(pt_string_inside,t_string_inside_length);
+        }
+    }
+}
+//比较并覆盖错误，讲低级错误转为字符串存放于描述中，
+//如果错误相同，则保留this的，将输入参数转入描述。
+void Error_manager::compare_and_cover_error( Error_manager * p_error_manager)
+{
+	if(this->m_error_code == SUCCESS)
+	{
+		error_manager_reset(*p_error_manager);
+	}
+	else if (p_error_manager->m_error_code == SUCCESS)
+	{
+		//
+	}
+	else
+	{
+		Error_manager t_error_manager_new;
+		char* pt_string_inside = NULL;
+		int t_string_inside_length = 0;
+		if(this->m_error_level < p_error_manager->m_error_level)
+		{
+			t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + this->m_description_length;
+			pt_string_inside = (char*)malloc(t_string_inside_length);
+			translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+			error_manager_reset(*p_error_manager);
+			add_error_description(pt_string_inside,t_string_inside_length);
+		}
+		else
+		{
+			t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + p_error_manager->m_description_length;
+			pt_string_inside = (char*)malloc(t_string_inside_length);
+			p_error_manager->translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+			add_error_description(pt_string_inside,t_string_inside_length);
+		}
+	}
+}
+
+//将所有的错误信息，格式化为字符串，用作日志打印。
+//output：p_error_description     错误汇总的字符串指针，不可以为NULL，必须要有实际的内存
+//output:description_length      错误汇总的字符串长度，不可以为0，长度最好足够大，一般256即可。
+void Error_manager::translate_error_to_string(char* p_error_aggregate, int aggregate_length )
+{
+    char t_string_array[ERROR_NAMAGER_TO_STRING_FRONT_LENGTH] = {0};
+    char* pt_index_front = t_string_array;
+    char* pt_index_back = pm_error_description;
+    char* pt_index = p_error_aggregate;
+
+    sprintf(t_string_array, "error_code:0x%08x, error_level:%02d, error_description:", m_error_code , m_error_level );
+
+    int t_length_min = m_description_length + ERROR_NAMAGER_TO_STRING_FRONT_LENGTH -1;
+    if(t_length_min > aggregate_length)
+    {
+        t_length_min = aggregate_length;
+    }
+
+    for(int i=0;i<t_length_min; i++)
+    {
+        if(i < ERROR_NAMAGER_TO_STRING_FRONT_LENGTH -1)
+        {
+            *pt_index = *pt_index_front;
+            pt_index++;
+            pt_index_front++;
+        }
+        else
+        {
+            *pt_index = *pt_index_back;
+            pt_index++;
+            pt_index_back++;
+        }
+    }
+}
+//output：error_description_string     错误汇总的string
+void Error_manager::translate_error_to_string(std::string & error_aggregate_string)
+{
+    char t_string_array[ERROR_NAMAGER_TO_STRING_FRONT_LENGTH] = {0};
+    sprintf(t_string_array, "error_code:0x%08x, error_level:%02d, error_description:", m_error_code , m_error_level );
+
+    error_aggregate_string = t_string_array ;
+    if(pm_error_description != NULL)
+    {
+        error_aggregate_string += pm_error_description;
+    }
+    else
+    {
+        //error_aggregate_string += "NULL";
+    }
+}
+//错误码转字符串的简易版，可支持cout<<
+//return     错误汇总的string
+std::string Error_manager::to_string()
+{
+    std::string t_string;
+    translate_error_to_string(t_string);
+    return t_string;
+}
+
+
+
+
+//释放错误描述的内存，
+void Error_manager::free_description()
+{
+    if(pm_error_description != NULL)
+    {
+        free (pm_error_description);
+        pm_error_description = NULL;
+    }
+    m_description_length = 0;
+}
+
+//重新分配错误描述的内存，并从外部拷贝新的（深拷贝）
+//input：p_error_description     错误描述的字符串指针，可以为NULL，
+//input:description_length      错误描述的字符串长度，如果为0，则从p_error_description里面获取有效的长度
+void Error_manager::reallocate_memory_and_copy_string(const char* p_error_description, int description_length)
+{
+    free_description();
+
+    if(p_error_description != NULL)
+    {
+        char* pt_source = (char *)p_error_description;
+        char* pt_destination = NULL;
+
+        if(description_length == 0)
+        {
+            m_description_length = 0;
+            while (*pt_source != '\0')
+            {
+                m_description_length++;
+                pt_source++;
+            }
+            m_description_length++;
+            pt_source = (char *)p_error_description;
+        }
+        else
+        {
+            m_description_length = description_length;
+        }
+
+        pm_error_description =  (char*) malloc(m_description_length );
+        pt_destination = pm_error_description;
+
+        for(int i=0;i<m_description_length; i++)
+        {
+            *pt_destination = *pt_source;
+            pt_destination++;
+            pt_source++;
+        }
+    }
+
+    return;
+}
+
+
+//重新分配错误描述的内存，并从外部拷贝新的（深拷贝）
+//input：error_aggregate_string     错误描述的string
+void Error_manager::reallocate_memory_and_copy_string(std::string & error_aggregate_string)
+{
+    free_description();
+
+    if( ! error_aggregate_string.empty())
+    {
+        m_description_length = error_aggregate_string.length()+1;
+
+        pm_error_description =  (char*) malloc( m_description_length );
+
+        strcpy(pm_error_description ,   error_aggregate_string.c_str()  );
+    }
+}
+
+
+
+
+

src/agv_bundle/src/error_code/error_code.h

@@ -0,0 +1,396 @@
+
+//Error_code是错误码的底层通用模块，
+//功能：用作故障分析和处理。
+
+//用法：所有的功能接口函数return错误管理类，
+//然后上层判断分析错误码，并进行故障处理。
+
+
+
+#ifndef TEST_ERROR_ERROR_CODE_H
+#define TEST_ERROR_ERROR_CODE_H
+
+#include <string>
+#include <string.h>
+
+//错误管理类转化为字符串 的前缀，固定长度为58
+//这个是由显示格式来确定的，如果要修改格式或者 Error_code长度超过8位，Error_level长度超过2位，折需要重新计算
+#define ERROR_NAMAGER_TO_STRING_FRONT_LENGTH   58
+
+//进程加锁的状态，
+enum Lock_status
+{
+    UNLOCK      = 0,
+    LOCK        = 1,
+};
+
+//设备使能状态，
+enum Able_status
+{
+    UNABLE      = 0,
+    ENABLE      = 1,
+};
+
+//数据是否为空
+enum Empty_status
+{
+    NON_EMPTY   = 0,
+    EMPTY       = 1,
+};
+
+
+//错误码的枚举，用来做故障分析
+enum Error_code
+{
+    //成功，没有错误，默认值0
+    SUCCESS                         = 0x00000000,
+
+
+    //基本错误码，
+    ERROR                           = 0x00000001,//错误
+    PARTIAL_SUCCESS                 = 0x00000002,//部分成功
+    WARNING                         = 0x00000003,//警告
+    FAILED                          = 0x00000004,//失败
+
+    NODATA                          = 0x00000010,//没有数据，传入参数容器内部没有数据时，
+
+    POINTER_IS_NULL                 = 0x00000101,//空指针
+    PARAMETER_ERROR                 = 0x00000102,//参数错误，传入参数不符合规范时，
+    POINTER_MALLOC_FAIL             = 0x00000103,//手动分配内存失败
+
+    CLASS_BASE_FUNCTION_CANNOT_USE  = 0x00000201,//基类函数不允许使用，必须使用子类的
+
+
+//    错误码的规范，
+//    错误码是int型，32位，十六进制。
+//    例如0x12345678
+//    12表示功能模块，例如:laser雷达模块               框架制定
+//    34表示文件名称，例如:laser_livox.cpp             框架制定
+//    56表示具体的类，例如:class laser_livox           个人制定
+//    78表示类的函数，例如:laser_livox::start();       个人制定
+//    注：错误码的制定从1开始，不要从0开始，
+//        0用作错误码的基数，用来位运算，来判断错误码的范围。
+
+//    laser扫描模块
+    LASER_ERROR_BASE                = 0x01000000,
+
+//    laser_base基类
+	LASER_BASE_ERROR_BASE			= 0x01010000,
+    LASER_TASK_PARAMETER_ERROR      = 0x01010001,   //雷达任务输入参数错误
+    LASER_CONNECT_FAILED,
+	LASER_START_FAILED,
+	LASER_CHECK_FAILED				,
+	LASER_STATUS_ERROR,								//雷达状态错误
+
+
+    LASER_LIVOX_SKD_INIT_FAILED,
+
+
+//    laser_livox.cpp的错误码
+    LIVOX_ERROR_BASE                = 0x01020000,
+    LIVOX_START_FAILE               = 0x01020101,
+	LIVOX_TASK_TYPE_ERROR,							//livox任务类型错误
+
+
+     //PLC error code  ...
+    PLC_ERROR_BASE                  = 0x02010000,
+    PLC_UNKNOWN_ERROR,
+    PLC_EMPTY_TASK,
+    PLC_IP_PORT_ERROR,
+    PLC_SLAVE_ID_ERROR,
+    PLC_CONNECTION_FAILED,
+    PLC_READ_FAILED,
+    PLC_WRITE_FAILED,
+    PLC_NOT_ENOUGH_DATA_ERROR,
+
+    //Locater.cpp error from 0x0301000-0x030100FF
+        LOCATER_TASK_INIT_CLOUD_EMPTY=0x03010000,
+    LOCATER_TASK_ERROR,
+    LOCATER_TASK_INPUT_CLOUD_UNINIT,
+    LOCATER_INPUT_CLOUD_EMPTY,
+    LOCATER_YOLO_UNINIT,
+    LOCATER_POINTSIFT_UNINIT,
+    LOCATER_3DCNN_UNINIT,
+    LOCATER_INPUT_YOLO_CLOUD_EMPTY,
+    LOCATER_Y_OUT_RANGE_BY_PLC,
+    LOCATER_MEASURE_HEIGHT_CLOUD_UNINIT,
+    LOCATER_MEASURE_HEIGHT_CLOUD_EMPTY,
+    LOCATER_INPUT_CLOUD_UNINIT,
+
+
+    //point sift from 0x03010100-0x030101FF
+        LOCATER_SIFT_INIT_FAILED=0x03010100,
+    LOCATER_SIFT_INPUT_CLOUD_UNINIT,
+    LOCATER_SIFT_PREDICT_FAILED,
+    LOCATER_SIFT_CREATE_INPUT_DATA_FAILED,
+    LOCATER_SIFT_FILTE_OBS_FAILED,
+    LOCATER_SIFT_INPUT_BOX_PARAMETER_FAILED,
+    LOCATER_SIFT_INPUT_CLOUD_EMPTY,
+    LOCATER_SIFT_PREDICT_NO_CAR_POINT,
+
+    //yolo from 0x03010200-0x030102FF
+        LOCATER_YOLO_DETECT_FAILED=0x03010200,
+    LOCATER_YOLO_DETECT_NO_TARGET,
+    LOCATER_YOLO_PARAMETER_INVALID,
+    LOCATER_YOLO_INPUT_CLOUD_UNINIT,
+
+    //3dcnn from 0x03010300-0x030103FF
+    LOCATER_3DCNN_INIT_FAILED=0x03010300,
+    LOCATER_3DCNN_INPUT_CLOUD_UNINIT,
+    LOCATER_3DCNN_PREDICT_FAILED,
+    LOCATER_3DCNN_VERIFY_RECT_FAILED_3,
+    LOCATER_3DCNN_VERIFY_RECT_FAILED_4,
+    LOCATER_3DCNN_KMEANS_FAILED,
+    LOCATER_3DCNN_IIU_FAILED,
+    LOCATER_3DCNN_INPUT_CLOUD_EMPTY,
+    LOCATER_3DCNN_PCA_OUT_CLOUD_EMPTY,
+
+    //System_manager error from 0x04010000-0x0401FFFF
+    SYSTEM_READ_PARAMETER_ERROR=0x04010100,
+    SYSTEM_PARAMETER_ERROR,
+    SYSTEM_INPUT_TERMINOR_NO_LASERS,
+
+    //terminor_command_executor.cpp from 0x04010200-0x040102FF
+    TERMINOR_NOT_READY=0x04010200,
+    TERMINOR_INPUT_LASER_NULL,
+    TERMINOR_NOT_CONTAINS_LASER,
+    TERMINOR_INPUT_PLC_NULL,
+    TERMINOR_INPUT_LOCATER_NULL,
+    TERMINOR_CREATE_WORKING_THREAD_FAILED,
+    TERMINOR_FORCE_QUIT,
+    TERMINOR_LASER_TIMEOUT,
+    TERMINOR_POST_PLC_TIMEOUT,
+    TERMINOR_CHECK_RESULTS_ERROR,
+
+    ////Hardware limit from 0x05010000 - 0x0501ffff
+    ///railing.cpp from 0x05010100-0x050101ff
+    HARDWARE_LIMIT_LEFT_RAILING=0x05010100,         //左栏杆限制
+    HARDWARE_LIMIT_RAILING_PARAMETER_ERROR,
+    HARDWARE_LIMIT_RAILING_ERROR,
+    HARDWARE_LIMIT_CENTER_X_LEFT,
+    HARDWARE_LIMIT_CENTER_X_RIGHT,
+    HARDWARE_LIMIT_CENTER_Y_TOP,
+    HARDWARE_LIMIT_CENTER_Y_BOTTOM,
+    HARDWARE_LIMIT_HEIGHT_OUT_RANGE,
+    HARDWARE_LIMIT_ANGLE_OUT_RANGE,
+    //termonal_limit from 0x05010200-0x050102ff
+    HARDWARE_LIMIT_TERMINAL_LEFT_ERROR,
+    HARDWARE_LIMIT_TERMINAL_RIGHT_ERROR,
+    HARDWARE_LIMIT_TERMINAL_LR_ERROR,
+
+
+    //wj_lidar error from 0x06010000-0x0601FFFF
+        WJ_LIDAR_CONNECT_FAILED=0x06010000,
+    WJ_LIDAR_UNINITIALIZED,
+    WJ_LIDAR_READ_FAILED,
+    WJ_LIDAR_WRITE_FAILED,
+    WJ_LIDAR_GET_CLOUD_TIMEOUT,
+
+    //wj lidar protocol error from 0x06020000-0x0602FFFF
+        WJ_PROTOCOL_ERROR_BASE=0x06020000,
+    WJ_PROTOCOL_INTEGRITY_ERROR,
+    WJ_PROTOCOL_PARSE_FAILED,
+    WJ_PROTOCOL_EMPTY_PACKAGE,
+    WJ_PROTOCOL_EXCEED_MAX_SIZE,
+
+    //wj region detect error from 0x06030000-0x0603FFFF
+        WJ_REGION_EMPTY_CLOUD=0x06030000,
+    WJ_REGION_RECTANGLE_ANGLE_ERROR,
+    WJ_REGION_RECTANGLE_SIZE_ERROR,
+    WJ_REGION_RECTANGLE_SYMMETRY_ERROR,
+    WJ_REGION_CLUSTER_SIZE_ERROR,
+
+    //wj manager error from 0x06040000-0x0604FFFF
+    WJ_MANAGER_UNINITIALIZED=0x06040000,
+    WJ_MANAGER_LIDAR_DISCONNECTED,
+    WJ_MANAGER_PLC_DISCONNECTED,
+    WJ_MANAGER_EMPTY_CLOUD,
+
+    WJ_LIDAR_TASK_EMPTY_RESULT=0x06050000,
+    WJ_LIDAR_TASK_EMPTY_TASK,
+    WJ_LIDAR_TASK_WRONG_TYPE,
+    WJ_LIDAR_TASK_INVALID_TASK,
+    WJ_LIDAR_TASK_MEASURE_FAILED,
+
+    //cifx error from 0x07010000-0x0701FFFF
+    CIFX_DRIVER_INIT_ERROR=0x07010000,
+    CIFX_DRIVER_OPEN_ERROR,
+    CIFX_CHANNEL_OPEN_ERROR,
+    CIFX_BUS_OPEN_ERROR,
+    CIFX_READ_FAILED,
+    CIFX_READ_TIMEOUT,
+    CIFX_WRITE_FAILED,
+    CIFX_WRITE_TIMEOUT,
+
+    //odom error from 0x07020000-0x0702FFFF
+    
+};
+
+//错误等级，用来做故障处理
+enum Error_level
+{
+//    正常，没有错误，默认值0
+    NORMAL                = 0,
+
+
+//    可忽略的故障，NEGLIGIBLE_ERROR
+//    提示作用，不做任何处理，不影响代码的流程，
+//    用作一些不重要的事件，一半出错也不会影响到系统功能，
+//    例如：文件保存错误，等
+    NEGLIGIBLE_ERROR      = 1,
+
+
+//    一般故障，MINOR_ERROR
+//    用作底层功能函数的错误返回，表示该功能函数执行失败，
+//    返回给应用层之后，需要做故障分析和处理，
+//    例如：雷达数据传输失败，应用层进行重新扫描，或者重连，或者重置参数等。
+    MINOR_ERROR           = 2,
+
+
+//    严重故障，MAJOR_ERROR
+//    用作应用层的任务事件的结果，表示该功能模块失败。
+//    通常是底层函数返回一般故障之后，应用层无法处理并解决故障，此时就要进行故障升级，
+//    从一般故障升级为严重故障，然后进行回退流程，回退已经执行的操作，最终回到故障待机状态。
+//    需要外部清除故障，并复位至正常待机状态，才能恢复功能的使用。
+//    例如：雷达扫描任务失败，且无法自动恢复。
+    MAJOR_ERROR           = 3,
+
+
+//    致命故障，CRITICAL_ERROR
+//    系统出现致命错误。导致系统无法正常运行，
+//    此时系统应该紧急停机，执行紧急流程，快速停机。
+//    此时不允许再执行任何函数和任务指令，防止系统故障更加严重。
+//    也不需要做任何错误处理了，快速执行紧急流程。
+//    例如：内存错误，进程挂死，关键设备失控，监控设备报警，等
+    CRITICAL_ERROR        = 4,
+};
+
+
+class Error_manager
+{
+public://外部接口函数
+    //构造函数
+    Error_manager();
+    //拷贝构造
+    Error_manager(const Error_manager & error_manager);
+    //赋值构造
+    Error_manager(Error_code error_code, Error_level error_level = NORMAL,
+                  const char* p_error_description = NULL, int description_length = 0);
+    //赋值构造
+    Error_manager(Error_code error_code, Error_level error_level , std::string & error_aggregate_string);
+    //析构函数
+    ~Error_manager();
+
+    //初始化
+    void error_manager_init();
+    //初始化
+    void error_manager_init(Error_code error_code, Error_level error_level = NORMAL,
+                            const char* p_error_description = NULL, int description_length = 0);
+    //初始化
+    void error_manager_init(Error_code error_code, Error_level error_level , std::string & error_aggregate_string);
+    //重置
+    void error_manager_reset(Error_code error_code, Error_level error_level = NORMAL,
+                             const char* p_error_description = NULL, int description_length = 0);
+    //重置
+    void error_manager_reset(Error_code error_code, Error_level error_level , std::string & error_aggregate_string);
+    //重置
+    void error_manager_reset(const Error_manager & error_manager);
+    //清除所有内容
+    void error_manager_clear_all();
+
+    //重载=
+    Error_manager& operator=(const Error_manager & error_manager);
+    //重载=，支持Error_manager和Error_code的直接转化，会清空错误等级和描述
+    Error_manager& operator=(Error_code error_code);
+    //重载==
+    bool operator==(const Error_manager & error_manager);
+    //重载==，支持Error_manager和Error_code的直接比较
+    bool operator==(Error_code error_code);
+    //重载!=
+    bool operator!=(const Error_manager & error_manager);
+    //重载!=，支持Error_manager和Error_code的直接比较
+    bool operator!=(Error_code error_code);
+
+
+    //获取错误码
+    Error_code get_error_code();
+    //获取错误等级
+    Error_level get_error_level();
+    //获取错误描述的指针，（浅拷贝）
+    char* get_error_description();
+
+    //复制错误描述，（深拷贝）
+    //output：p_error_description     错误描述的字符串指针，不可以为NULL，必须要有实际的内存
+    //output:description_length      错误描述的字符串长度，不可以为0，长度最好足够大，一般256即可。
+    void copy_error_description(const char* p_error_description, int description_length);
+    //复制错误描述，（深拷贝）
+    //output：error_description_string     错误描述的string
+    void copy_error_description(std::string & error_description_string);
+
+    //设置错误码
+    void set_error_code(Error_code error_code);
+    //比较错误等级并升级，取高等级的结果
+    void set_error_level_up(Error_level error_level);
+    //比较错误等级并降级，取低等级的结果
+    void set_error_level_down(Error_level error_level);
+    //错误等级，设定到固定值
+    void set_error_level_location(Error_level error_level);
+    //设置错误描述
+    void set_error_description(const char* p_error_description, int description_length = 0);
+    //设置错误描述
+    void set_error_description(std::string & error_description_string);
+
+    //尾部追加错误描述
+    void add_error_description(const char* p_error_description, int description_length = 0);
+    //尾部追加错误描述
+    void add_error_description(std::string & error_description_string);
+
+    //比较错误是否相同，
+    // 注：只比较错误码和等级
+	bool is_equal_error_manager(const Error_manager & error_manager);
+	//比较并覆盖错误，讲低级错误转为字符串存放于描述中，
+	//如果错误相同，则保留this的，将输入参数转入描述。
+	void compare_and_cover_error(const Error_manager & error_manager);
+	//比较并覆盖错误，讲低级错误转为字符串存放于描述中，
+	//如果错误相同，则保留this的，将输入参数转入描述。
+	void compare_and_cover_error( Error_manager * p_error_manager);
+
+	//将所有的错误信息，格式化为字符串，用作日志打印。
+    //output：p_error_description     错误汇总的字符串指针，不可以为NULL，必须要有实际的内存
+    //output:description_length      错误汇总的字符串长度，不可以为0，长度最好足够大，一般256即可。
+    void translate_error_to_string(char* p_error_aggregate, int aggregate_length);
+    //output：error_description_string     错误汇总的string
+    void translate_error_to_string(std::string & error_aggregate_string);
+    //错误码转字符串的简易版，可支持cout<<
+    //return     错误汇总的string
+    std::string to_string();
+
+
+
+protected:
+    Error_code              m_error_code;               //错误码
+    Error_level             m_error_level;              //错误等级
+    char*                   pm_error_description;       //错误描述
+    int                     m_description_length;       //错误描述的字符长度
+
+protected://内部功能函数
+public:
+    //释放错误描述的内存，
+    void free_description();
+
+    //重新分配错误描述的内存，并从外部拷贝新的（深拷贝）
+    //input：p_error_description     错误描述的字符串指针，可以为NULL，
+    //input:description_length      错误描述的字符串长度，如果为0，则从p_error_description里面获取有效的长度
+    void reallocate_memory_and_copy_string(const char* p_error_description, int description_length = 0);
+
+    //重新分配错误描述的内存，并从外部拷贝新的（深拷贝）
+    //input：error_aggregate_string     错误描述的string
+    void reallocate_memory_and_copy_string(std::string & error_aggregate_string);
+};
+
+
+
+
+#endif //TEST_ERROR_ERROR_CODE_H
+
+

src/agv_bundle/src/locate/ceres_scan_match.cpp

@@ -0,0 +1,319 @@
+/*
+ * @Description: match scan with grid map
+ * @Author: yct
+ * @Date: 2020-04-03 13:10:37
+ * @LastEditTime: 2020-06-21 23:56:17
+ * @LastEditors: yct
+ */
+
+#include "ceres_scan_match.h"
+
+bool read_pointcloud(std::string filename, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
+{
+    // std::lock_guard<std::mutex> lck(m_cloud_mutex);
+    pointcloud->clear();
+    std::ifstream in(filename);
+    if (!in.is_open())
+    {
+        std::cout << "failed to open file " << filename << std::endl;
+        return false;
+    }
+    while (!in.eof())
+    {
+        std::string t_linestr;
+        if (getline(in, t_linestr))
+        {
+            std::vector<std::string> str_vector;
+            std::stringstream ss(t_linestr);
+            std::string num_str;
+            while (getline(ss, num_str, ' '))
+            {
+                str_vector.push_back(num_str);
+            }
+            if (str_vector.size() != 3)
+            {
+                std::cout << "unsupported point cloud / cannot find point x y z value: " << filename << std::endl;
+                return false;
+            }
+            pcl::PointXYZ t_cloud;
+            t_cloud.x = stod(str_vector[0]);
+            t_cloud.y = stod(str_vector[1]);
+            t_cloud.z = stod(str_vector[2]);
+            pointcloud->push_back(t_cloud);
+        }
+    }
+    in.close();
+    std::cout << "file read finished with points " << pointcloud->size() << std::endl;
+    return true;
+}
+
+Ceres_scan_match::Ceres_scan_match(std::string filename, Eigen::Vector3d trans)
+{
+    // m_odom_pub = m_nh.advertise<nav_msgs::Odometry>("odom", 1, true);
+    m_scan_subscriber = m_nh.subscribe("scan", 1, &Ceres_scan_match::scan_callback, this);
+    m_initial_pose_subscriber = m_nh.subscribe("/initialpose", 1, &Ceres_scan_match::initial_pose_callback, this);
+    m_cloud_publisher = m_nh.advertise<sensor_msgs::PointCloud2>("match_cloud", 10);
+    m_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    //read_pointcloud("/home/youchen/Documents/git_ws/src/map_scan_match/my1.txt", m_cloud);
+    m_trans = trans;
+    // m_trans << 0.0, 0.0, 0.0;
+    if (read_map(filename))
+    {
+        m_probability_array.rows = m_global_map.rows;
+        m_probability_array.cols = m_global_map.cols;
+        for (size_t i = 0; i < m_global_map.rows; i++)
+        {
+            for (size_t j = 0; j < m_global_map.cols; j++)
+            {
+                // 处理原始图片空白区域
+                double value = static_cast<double>(m_global_map.at<float>(i, j));
+                if(value < 0.51 && value > 0.49)
+                    value = 0.9;
+                m_probability_array.m_probability_vector.push_back(value);
+            }  
+        }
+        m_initialized = true;
+        std::cout << "initialized" << std::endl;
+    }
+    else
+    {
+        m_initialized = false;
+    }
+    m_initial_pose_updated = false;
+    m_initial_pose.setIdentity();
+
+}
+
+Ceres_scan_match::~Ceres_scan_match()
+{
+}
+
+#include <pcl/common/transforms.h>
+bool Ceres_scan_match::Solve(Eigen::Vector3d &trans, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::Mat global_map)
+{
+    // std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
+    // trans[0] += m_map_origin[0];
+    // trans[1] += m_map_origin[1];
+    // double tr[3] = {0.0, 0.0, 0.0};
+    // std::cout<<"we get points: "<< cloud->size()<<std::endl;
+    ceres::Problem problem;
+    problem.AddResidualBlock(
+        new ceres::AutoDiffCostFunction<BicubicInterpolationCost, ceres::DYNAMIC, 3>(
+            new BicubicInterpolationCost(m_probability_array, cloud, m_resolution, m_map_origin, 50.0 / sqrt(static_cast<double>(cloud->size()))), cloud->size()),
+        nullptr,
+        trans.data());
+
+    problem.AddResidualBlock(
+        new ceres::AutoDiffCostFunction<TranslationCost, 2, 3>(
+            new TranslationCost(Eigen::Vector2d(trans[0], trans[1]), 10.0)),
+        nullptr,
+        trans.data());
+
+    problem.AddResidualBlock(
+        new ceres::AutoDiffCostFunction<RotationCost, 1, 3>(
+            new RotationCost(trans[2], 10.0)),
+        nullptr,
+        trans.data());
+
+    ceres::Solver::Options options;
+    options.linear_solver_type = ceres::DENSE_QR;
+    options.minimizer_progress_to_stdout = false;
+    options.max_num_iterations = 20;
+    options.num_threads = 1;
+    // options.parameter_tolerance = 1e-6;
+    // options.function_tolerance = 1e-6;
+    ceres::Solver::Summary summary;
+    ceres::Solve(options, &problem, &summary);
+    std::cout << summary.BriefReport() << std::endl;
+
+    // std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
+    // int duration = std::chrono::duration_cast<std::chrono::microseconds>(t1 - t0).count();
+    // std::cout << "real solve time us: " << duration << std::endl;
+
+    // 发布点云
+    // trans[0] -= m_map_origin[0];
+    // trans[1] -= m_map_origin[1];
+    std::cout << "final trans: "<< trans.transpose() << std::endl;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
+    transform_1.translation() << trans[0], trans[1], 0.0;
+    transform_1.rotate(Eigen::AngleAxisf(trans[2], Eigen::Vector3f::UnitZ()));
+    pcl::transformPointCloud(*cloud, *temp_cloud, transform_1);
+    sensor_msgs::PointCloud2 output_origin;
+    pcl::toROSMsg(*temp_cloud, output_origin);
+    output_origin.header.frame_id = "map";
+    m_cloud_publisher.publish(output_origin);
+
+    tf::TransformBroadcaster tf_broadcaster_;
+    tf::Transform transform;
+    transform.setOrigin(tf::Vector3(trans[0], trans[1], 0.0));
+    transform.setRotation(tf::Quaternion(0, 0, sin(trans[2]/2.0), cos(trans[2]/2.0)));
+    tf::StampedTransform transform_msg(transform, ros::Time::now(), "map", "laser");
+    tf_broadcaster_.sendTransform(transform_msg);
+
+
+    // // 发布里程计数据
+    // int period = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - m_tf_update_time).count();
+    // m_tf_update_time = std::chrono::steady_clock::now();
+    // static Eigen::Vector3d s_last_trans;
+    // Eigen::Vector3d delta_trans = trans - s_last_trans;
+    // s_last_trans = trans;
+    // nav_msgs::Odometry t_odom;
+    // t_odom.child_frame_id = "base_link";
+    // t_odom.header.frame_id = "map";
+    // t_odom.header.stamp = ros::Time::now();
+    // t_odom.pose.pose.position.x = trans[0];
+    // t_odom.pose.pose.position.y = trans[1];
+    // t_odom.pose.pose.position.z = 0;
+    // t_odom.pose.pose.orientation.x = 0.0;
+    // t_odom.pose.pose.orientation.y = 0.0;
+    // t_odom.pose.pose.orientation.z = sin(trans[2] / 2.0);
+    // t_odom.pose.pose.orientation.w = cos(trans[2] / 2.0);
+    // t_odom.twist.twist.linear.x = hypot(delta_trans[0], delta_trans[1]) / (period/1000.0);
+    // t_odom.twist.twist.angular.z = delta_trans[2] / (period/1000.0);
+    // std::cout << "odom: "<< t_odom.twist.twist.linear.x << ", " << t_odom.twist.twist.angular.z <<std::endl;
+    // m_odom_pub.publish(t_odom);
+}
+
+bool Ceres_scan_match::Solve()
+{
+    std::cout << "--- start solve ---" << std::endl;
+    std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
+    Eigen::Vector3d trans;
+    // if (m_initial_pose_updated)
+    // {
+        // 设置位姿初值
+        m_mutex.lock();
+        trans << m_initial_pose.getOrigin().getX(), m_initial_pose.getOrigin().getY(), asin(m_initial_pose.getRotation().getZ()) * 2.0;
+        std::cout << "init trans: "<< trans.transpose() << std::endl;
+        m_mutex.unlock();
+        m_initial_pose_updated = false;
+    // }else{
+        // trans << 1.5, -0.2, -0.75;
+        // trans << 0.7, 0.0, 0.1;
+    // }
+
+    // 点云抽析
+    pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
+    approximate_voxel_filter.setLeafSize(m_resolution, m_resolution, m_resolution);
+    approximate_voxel_filter.setInputCloud(m_cloud);
+    approximate_voxel_filter.filter(*m_cloud);
+    std::cout<<"cloud size: "<<m_cloud->size()<<std::endl;
+
+    // // 临时添加，测试carto地图用
+    // trans << 0.0, 0.0, 0.0;
+    // cv::Mat t_map = cv::imread("/home/youchen/Documents/git_ws/src/map_scan_match/my1.jpg", cv::IMREAD_GRAYSCALE);
+    // t_map.convertTo(t_map, CV_32FC1, 1.0 / 255.0);
+    // read_pointcloud("/home/youchen/Documents/git_ws/src/map_scan_match/my1.txt", m_cloud);
+
+    Solve(trans, m_cloud, m_global_map);
+    m_initial_pose.setOrigin(tf::Vector3(trans[0], trans[1], 0));
+    m_initial_pose.setRotation(tf::Quaternion(tf::Vector3(0,0,1), trans[2]));
+
+    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
+    int duration = std::chrono::duration_cast<std::chrono::microseconds>(t1 - t0).count();
+    std::cout << "solve time us: " << duration << std::endl;
+
+    // cv::Mat t_map = cv::imread("./src/map_scan_match/interpolate.jpg", cv::IMREAD_GRAYSCALE);
+    // cv::namedWindow("map", cv::WINDOW_FREERATIO);
+    // cv::imshow("map", t_map);
+    // cv::waitKey();
+}
+
+bool Ceres_scan_match::read_map(std::string conf_filename)
+{
+    std::cout << "conf_file: " << conf_filename << std::endl;
+    std::ifstream in(conf_filename);
+    if (!in.is_open())
+    {
+        std::cerr << "Failed to open " << conf_filename << std::endl;
+        return false;
+    }
+    while (!in.eof())
+    {
+        std::string t_line_str;
+        if (getline(in, t_line_str))
+        {
+            std::stringstream ss(t_line_str);
+            std::string t_str;
+            if (getline(ss, t_str, ' '))
+            {
+                if (t_str == "image:")
+                {
+                    m_map_path = t_line_str.substr(t_str.length() + 1, t_line_str.length() - t_str.length() - 1);
+                    m_global_map = cv::imread(m_map_path, cv::IMREAD_GRAYSCALE);
+                    // 图像归一化
+                    m_global_map.convertTo(m_global_map, CV_32FC1, 1.0 / 255.0);
+                    // // 值反转
+                    // m_global_map = cv::Mat::ones(m_global_map.size(), m_global_map.type()) - m_global_map;
+                    
+                    // std::cout<<m_global_map(cv::Range(0, 10), cv::Range(0, 10))<<std::endl;
+                    // std::cout<<cv::Scalar(m_global_map.at<uchar>(20, 12))<<std::endl;
+                    // std::cout<<cv::Scalar(m_global_map.at<uchar>(5, 8))<<std::endl;
+                    // cv::namedWindow("origin map", cv::WINDOW_FREERATIO);
+                    // cv::imshow("origin map", m_global_map);
+                }
+                else if (t_str == "resolution:")
+                {
+                    m_resolution = stod(t_line_str.substr(t_str.length() + 1, t_line_str.length() - t_str.length() - 1));
+                }
+                else if (t_str == "origin:")
+                {
+                    std::string t_origin_str = t_line_str.substr(t_str.length() + 2, t_line_str.length() - t_str.length() - 3);
+                    std::stringstream ss_origin(t_origin_str);
+                    std::string t_origin_value_str;
+                    for (size_t i = 0; i < 3; i++)
+                    {
+                        if (getline(ss_origin, t_origin_value_str, ','))
+                        {
+                            m_map_origin[i] = stod(t_origin_value_str);
+                        }
+                    }
+                }
+            }
+        }
+    }
+    // std::cout<<m_map_path<<std::endl;
+    // std::cout<<m_resolution<<std::endl;
+    // std::cout<<m_map_origin<<std::endl;
+    // cv::waitKey();
+    return true;
+}
+
+void Ceres_scan_match::scan_callback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
+{
+    if (scan_msg->ranges.size() <= 0)
+        return;
+    if (m_initialized)
+    {
+        std::cout << "-------------- scan callback --------------" << std::endl;
+        m_mutex.lock();
+        m_cloud->clear();
+        double l, angle;
+        // std::cout<<"scan callback1"<<std::endl;
+        for (size_t i = 0; i < scan_msg->ranges.size(); i++)
+        {
+            if(std::isnan(scan_msg->ranges[i])) continue;
+            // std::cout<<"scan callback2"<<std::endl;
+            // std::cout<<"scan callback3"<<scan_msg->intensities[0]<<std::endl;
+            l = scan_msg->ranges[i];
+            angle = scan_msg->angle_min + scan_msg->angle_increment * i;
+            // std::cout<<"scan callback4"<<std::endl;
+            m_cloud->push_back(pcl::PointXYZ(l * cos(angle), l * sin(angle), 0.0));
+            // std::cout<<"scan callback5"<<std::endl;
+        }
+        std::cout << "cloud size: " << m_cloud->size() << std::endl;
+        m_mutex.unlock();
+        Solve();
+    }
+}
+
+void Ceres_scan_match::initial_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &initial_pose_msg)
+{
+    geometry_msgs::Point position = initial_pose_msg->pose.pose.position;
+    geometry_msgs::Quaternion quat = initial_pose_msg->pose.pose.orientation;
+    std::lock_guard<std::mutex> lck(m_mutex);
+    m_initial_pose.setOrigin(tf::Vector3(position.x, position.y, position.z));
+    m_initial_pose.setRotation(tf::Quaternion(quat.x, quat.y, quat.z, quat.w));
+    m_initial_pose_updated = true;
+}

src/agv_bundle/src/locate/ceres_scan_match.h

@@ -0,0 +1,350 @@
+/*
+ * @Description: match scan with grid map, 监听scan与initpose话题，转成点云并发布
+ * @Author: yct
+ * @Date: 2020-04-03 11:50:01
+ * @LastEditTime: 2020-06-21 23:45:18
+ * @LastEditors: yct
+ */
+#ifndef CERES_SCAN_MATCH_HH
+#define CERES_SCAN_MATCH_HH
+
+#include <algorithm>
+#include <string>
+#include <iostream>
+#include <fstream>
+#include <atomic>
+#include <mutex>
+#include <cmath>
+#include <chrono>
+#include <ros/ros.h>
+#include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/point_cloud_conversion.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/filters/approximate_voxel_grid.h>
+#include <Eigen/Core>
+#include <opencv2/opencv.hpp>
+#include "ceres/ceres.h"
+#include "ceres/cubic_interpolation.h"
+
+#include <nav_msgs/Odometry.h>
+
+class prob_array
+{
+public:
+    prob_array():rows(0),cols(0){
+        m_probability_vector.clear();
+    }
+    void operator=(prob_array& other)
+    {
+        this->rows = other.rows;
+        this->cols = other.cols;
+        this->m_probability_vector.clear();
+        for (size_t i = 0; i < other.m_probability_vector.size(); i++)
+        {
+            this->m_probability_vector.push_back(other.m_probability_vector[i]);
+        }
+    }
+    // 列数
+    int rows;
+    // 行数
+    int cols;
+    // 像素值数组，用于计算加速
+    std::vector<double> m_probability_vector;
+};
+
+class Ceres_scan_match
+{
+public:
+
+    // 传入yaml地图配置文件
+    Ceres_scan_match(std::string filename, Eigen::Vector3d trans);
+    ~Ceres_scan_match();
+    bool read_map(std::string conf_filename);
+
+
+private:
+    bool Solve(Eigen::Vector3d &trans, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::Mat global_map);
+    bool Solve();
+    
+    void scan_callback(const sensor_msgs::LaserScan::ConstPtr &scan_msg);
+    void initial_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &initial_pose_msg);
+
+private:
+    // ros句柄
+    ros::NodeHandle m_nh;
+    ros::Subscriber m_scan_subscriber;
+    ros::Subscriber m_initial_pose_subscriber;
+    ros::Publisher m_cloud_publisher;
+    ros::Publisher m_odom_pub; // 发布里程计测试用
+    tf::TransformBroadcaster tf_broadcaster_;
+    std::atomic_bool m_initial_pose_updated;
+    tf::Pose m_initial_pose;
+    std::mutex m_mutex;
+
+    // cv全局地图
+    cv::Mat m_global_map;
+    // 图像分辨率
+    double m_resolution;
+    // 全局地图像素值数组，用于加速
+    prob_array m_probability_array;
+    // 当前待匹配点云
+    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud;
+    // 待求变换
+    Eigen::Vector3d m_trans;
+    // 初始化状态
+    std::atomic_bool m_initialized;
+    // 地图路径
+    std::string m_map_path;
+    // 地图中心点
+    Eigen::Vector3d m_map_origin;
+    // tf更新时间
+    std::chrono::steady_clock::time_point m_tf_update_time;
+};
+
+class BicubicInterpolationCost
+{
+public:
+    BicubicInterpolationCost(const prob_array& map, const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, const double resolution, const Eigen::Vector3d map_origin, const double scale)
+    :m_resolution(resolution),
+    m_scale(scale),
+    m_map_origin(map_origin),
+    m_prob_array(map)
+    {
+        // std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
+        // m_prob_array = map;
+        // m_resolution = resolution;
+        // m_scale = scale;
+        // m_map_origin = map_origin;
+        m_points.reserve(cloud->size());
+        for (size_t i = 0; i < cloud->size(); i++)
+        {
+            m_points.emplace_back(Eigen::Vector2d(cloud->at(i).x, cloud->at(i).y));
+        }
+        
+        // std::cout<<"*************************************************************************"<<std::endl;
+        // m_grid.reset(new ceres::Grid2D<double, 1>(
+        //     m_grid_values.data(), 0, m_map.rows*3, 0, m_map.cols*3));
+        // m_interpolator.reset(new ceres::BiCubicInterpolator<ceres::Grid2D<double, 1> >(*m_grid));
+
+        // generate_map_img();
+        // std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
+        // int duration = std::chrono::duration_cast<std::chrono::milliseconds>(t1 - t0).count();
+        // std::cout << "bicubic init time: " << duration << std::endl;
+    }
+
+    // debug用，生成差值后的地图供比对
+    void generate_map_img()
+    {
+        cv::Mat interpolate_map = cv::Mat::ones(m_prob_array.rows, m_prob_array.cols, CV_8UC1);
+        const GridArrayAdapter adapter(m_prob_array);
+        ceres::BiCubicInterpolator<GridArrayAdapter> t_interpolator(adapter);
+        // interpolate_map *= 50.0;
+        std::cout<<"*************************************************************************"<<std::endl;
+        for (size_t i = 0; i < m_prob_array.rows; i++)
+        {
+            for (size_t j = 0; j < m_prob_array.cols; j++)
+            {
+                double f;
+                t_interpolator.Evaluate(double(i+kPadding), double(j+kPadding), &f);
+                // std::cout<<i<<", "<<j<<"--- map value: "<< f <<std::endl;
+                interpolate_map.at<uchar>(i, j) = static_cast<uchar>(f*255.0);
+            }
+        }
+
+        // 将点云映射到图像中
+        Eigen::Matrix<double, 2, 1> translation(0.0, 0.0);
+        Eigen::Rotation2D<double> rotation(0.0);
+        Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+        Eigen::Matrix<double, 3, 3> transform;
+        transform << rotation_matrix, translation, double(0.), double(0.), double(1.);
+        for (size_t i = 0; i < m_points.size(); ++i)
+        {
+            // Note that this is a 2D point. The third component is a scaling factor.
+            const Eigen::Matrix<double, 3, 1> point(
+                double(m_points[i].x()),
+                double(m_points[i].y()),
+                double(1.)
+                );
+            const Eigen::Matrix<double, 3, 1> world = transform * point;
+            int row = (int)(m_prob_array.rows - ( - m_map_origin.y() + world[1] ) / m_resolution + 0.5);
+            int col = (int)(( - m_map_origin.x() + world[0] ) / m_resolution + 0.5);
+            interpolate_map.at<uchar>(row, col) = static_cast<uchar>(0);
+        }
+        
+        // cv::imwrite("./src/map_scan_match/interpolate.jpg", interpolate_map);
+        cv::namedWindow("map", cv::WINDOW_FREERATIO);
+        cv::imshow("map", interpolate_map);
+        cv::waitKey();
+    }
+
+    template <typename T>
+    bool operator()(const T *const pose, T *residual) const
+    {
+        Eigen::Matrix<T, 2, 1> translation(pose[0], pose[1]);
+        // Eigen::Matrix<T, 2, 1> translation(pose[0] - m_map_origin[0], pose[1] - m_map_origin[1]);
+        Eigen::Rotation2D<T> rotation(pose[2]);
+        Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+        Eigen::Matrix<T, 3, 3> transform;
+        transform << rotation_matrix, translation, T(0.), T(0.), T(1.);
+        
+        const GridArrayAdapter adapter(m_prob_array);
+        ceres::BiCubicInterpolator<GridArrayAdapter> t_interpolator(adapter);
+        // const MapLimits &limits = grid_.limits();
+        // std::cout<<"****************************"<<std::endl;
+        // std::cout<<"+++++++ (row, col): "<<m_map.rows<<" "<<m_map.cols<<std::endl;
+        // char res[2048];
+        // memset(res, 0, 2048);
+        // sprintf(res, "residual values:\n");
+        for (size_t i = 0; i < m_points.size(); ++i)
+        {
+            // Note that this is a 2D point. The third component is a scaling factor.
+            const Eigen::Matrix<T, 3, 1> point(
+                T(m_points[i].x()),
+                T(m_points[i].y()),
+                T(1.)
+                );
+            const Eigen::Matrix<T, 3, 1> world = transform * point;
+            t_interpolator.Evaluate(
+                (T)m_prob_array.rows - ( - m_map_origin.y() + world[1] ) / m_resolution + 0.5 + T(kPadding),
+                ( - m_map_origin.x() + world[0] ) / m_resolution + 0.5 + T(kPadding),
+                &residual[i]);
+            // t_interpolator.Evaluate(
+            //     (T(m_map.rows/2.0) - world[0] / m_resolution) - 0.5 + T(kPadding),
+            //     (T(m_map.cols/2.0) - world[1] / m_resolution) - 0.5 + T(kPadding),
+            //     &residual[i]);
+            residual[i] *= m_scale;
+            // if (i % 10 == 0)
+            // {
+            //     sprintf(res + strlen(res), "\n");
+            // }
+            // sprintf(res + strlen(res), "%.3f ", residual[i]);
+            // std::cout<<i<<"th point: "<<(-world[1]) / m_resolution - 0.5 + T(m_map.rows) - 60.0<<" "<<(width+world[0]) / m_resolution - 0.5 + T(m_map.cols)+75.0<<std::endl;
+            // std::cout<<i<<"th residual: "<<residual[i]<<std::endl;
+            // if(i%40==0)
+            // {
+            //     // std::cout<<"0 point: "<<m_points[i]<<std::endl;
+            //     // std::cout<<world[0]<<", "<<world[1]<<", "<< m_resolution << "." <<std::endl;
+            //     std::cout<<(T(m_map.rows) - (world[1]) / m_resolution) - 0.5<<",, "<<(world[0]) / m_resolution - 0.5<<",,"<<residual[i]<<std::endl;
+            // }
+        }
+        // std::cout<<res<<std::endl;
+        return true;
+    }
+
+    class GridArrayAdapter
+    {
+    public:
+        enum { DATA_DIMENSION = 1 };
+        explicit GridArrayAdapter(const prob_array& map) : m_grid_map(map) {
+        }
+
+        void GetValue(const int row, const int column, double *const value) const
+        {
+            if (m_grid_map.m_probability_vector.empty() || row < kPadding || row >= NumRows() - kPadding || column < kPadding || column >= NumCols() - kPadding)
+            {
+                *value = 0.9;
+                return;
+            }
+            *value = m_grid_map.m_probability_vector[(row - kPadding) * m_grid_map.cols + column - kPadding];
+            // std::cout<<"rc value"<<*value<<std::endl;
+        }
+
+        int NumRows() const
+        {
+            return m_grid_map.rows + 2 * kPadding;
+        }
+
+        int NumCols() const
+        {
+            return m_grid_map.cols + 2 * kPadding;
+        }
+
+    private:
+        const prob_array& m_grid_map;
+    };
+private:
+    double m_scale;
+    // 边界
+    static constexpr int kPadding = INT_MAX / 4;
+    // 概率地图结构体
+    const prob_array& m_prob_array;
+    // 点坐标数组
+    std::vector<Eigen::Vector2d> m_points;
+    // 地图原始分辨率
+    double m_resolution;
+    // 地图原点
+    Eigen::Vector3d m_map_origin;
+    // // ceres二维地图类
+    // std::unique_ptr<ceres::Grid2D<double, 1> > m_grid;
+    // // ceres插值器
+    // std::unique_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<double, 1> > > m_interpolator;
+};
+
+/**
+ * @description: 平移代价函数类
+ */
+class TranslationCost
+{
+public:
+    TranslationCost(Eigen::Vector2d translation, double translation_weight)
+    {
+        m_translation = translation;
+        m_translation_weight = translation_weight;
+    }
+    ~TranslationCost()
+    {
+        
+    }
+
+    template <typename T>
+    bool operator()(const T *const pose, T *residual) const
+    {
+        residual[0] = m_translation_weight * (pose[0] - m_translation[0]);
+        residual[1] = m_translation_weight * (pose[1] - m_translation[1]);
+        return true;
+    }
+
+private:
+    // 原始平移
+    Eigen::Vector2d m_translation;
+    double m_translation_weight;
+};
+
+/**
+ * @description: 旋转代价函数类
+ */
+class RotationCost
+{
+public:
+    RotationCost(double angle, double rotation_weight)
+    {
+        m_angle = angle;
+        m_rotation_weight = rotation_weight;
+    }
+    ~RotationCost()
+    {
+
+    }
+
+    template <typename T>
+    bool operator()(const T *const pose, T *residual) const
+    {
+        residual[0] = m_rotation_weight * (pose[2] - m_angle);
+        return true;
+    }
+private:
+    // 原始旋转
+    double m_angle;
+    double m_rotation_weight;
+};
+
+
+#endif // !CERES_SCAN_MATCH_HH

src/agv_bundle/src/moving_control/FourthTrajectory.cpp

@@ -0,0 +1,149 @@
+//
+// Created by zx on 2019/8/31.
+//
+
+#include "FourthTrajectory.h"
+
+FourthTrajectory::FourthTrajectory()
+{
+
+}
+FourthTrajectory::~FourthTrajectory()
+{}
+
+bool FourthTrajectory::Make(tf::Transform t0, tf::Transform t1, int extend_num)
+{
+    position0_=t0;
+    position1_=t1;
+
+    double angle =gen_axis_angle();
+
+    tf::Transform pose1=Rotate_pose(position0_,angle);
+    tf::Transform pose2=Rotate_pose(position1_,angle);
+    double gradient1=get_gradient_by_tf(pose1);
+    double gradient2=get_gradient_by_tf(pose2);
+
+    //std::cout<<"grad1 grad2 :"<<gradient1<<" ,"<<gradient2<<std::endl;
+    //三阶多项式差值，生成 曲线
+    double x0=pose1.getOrigin().getX();
+    double y0=pose1.getOrigin().getY();
+    double x1=pose2.getOrigin().getX();
+    double y1=pose2.getOrigin().getY();
+
+    //std::cout<<" x1 ,x2 : "<<x0<<" , "<<x1<<std::endl;
+
+    Eigen::Matrix<double,5,5> A=Eigen::MatrixXd::Zero(5,5);    //系数矩阵A
+    Eigen::Matrix<double,5,1> B=Eigen::MatrixXd::Zero(5,1);    //值矩阵B
+    A(0,0)=pow(x0,4.0);
+    A(0,1)=pow(x0,3.0);
+    A(0,2)=pow(x0,2.0);
+    A(0,3)=x0;
+    A(0,4)=1.0;
+
+    A(1,0)=pow(x1,4.0);
+    A(1,1)=pow(x1,3.0);
+    A(1,2)=pow(x1,2.0);
+    A(1,3)=x1;
+    A(1,4)=1.0;
+
+    A(2,0)=4.0*pow(x0,3.0);
+    A(2,1)=3.0*x0*x0;
+    A(2,2)=2.0*x0;
+    A(2,3)=1.0;
+
+    A(3,0)=4.0*pow(x1,3.0);
+    A(3,1)=3.0*x1*x1;
+    A(3,2)=2.0*x1;
+    A(3,3)=1.0;
+
+    A(4,0)=12*pow(x1,2.0);
+    A(4,1)=6.0*x1;
+    A(4,2)=2.0;
+
+    B(0,0)=y0;
+    B(1,0)=y1;
+    B(2,0)=gradient1;
+    B(3,0)=gradient2;
+    B(4,0)=0;
+    Eigen::MatrixXd P=A.inverse()*B;
+    line_param_=P;
+
+    double a=P(0,0);
+    double b=P(1,0);
+    double c=P(2,0);
+    double d=P(3,0);
+    double e=P(4,0);
+    //生成离散点,间距 step (m)
+
+    boost::mutex::scoped_lock lock(mutex_trajectory_);
+    trajectory_.clear();
+
+    double delta=0.05;
+    double x=x0;
+    // added by yct
+    int extend_front_count = 0;
+    int extend_end_count = 0;
+    int sign = 1;
+    //modified by yct
+    while(x<x1 || (extend_num>0 && extend_end_count<=extend_num))
+    {
+        double y=a*pow(x,4.0)+b*pow(x,3.0)+c*x*x+d*x+e;
+        double gradient=4.0*a*pow(x,3.0)+3.0*b*x*x+2.0*c*x+d;
+
+        //计算x处的角度变化率
+        /*double gradtheta=threta_gradient(x);
+        if(fabs(gradtheta)>1.0/distance)
+        {
+            trajectory_.clear();
+            return false;
+        }*/
+
+        tf::Transform pose;
+        pose.setOrigin(tf::Vector3(x,y,0));
+        pose=get_tf(x,y,gradient);
+
+        // modified by yct
+        // 开头逆向移动x到扩展头，再开始入队.
+        if (extend_num > 0)
+        {
+            if(extend_front_count <= extend_num)
+            {
+                extend_front_count++;
+                sign = -1;
+            }else{
+                sign = 1;
+                trajectory_.push_back(pose);
+                if(x>=x1 && extend_end_count <= extend_num){
+                    extend_end_count++;
+                }
+            }
+        }
+        else{
+            trajectory_.push_back(pose);
+        }
+
+        double delta_x=sqrt(delta*delta/(1.0+gradient*gradient));
+        // modified by yct
+        x+=delta * sign;
+    }
+
+    //最后一点加入轨迹中, modified by yct, 非扩展手动加最后一点
+    if (extend_num <= 0)
+    {
+        double x_last = x1;
+        double y_last = a * pow(x_last, 4.0) + b * pow(x_last, 3.0) + c * x_last*x_last + d*x_last+e;
+        double gradient_last = 4.0 * a * pow(x_last, 3.0) + 3.0 * b * x_last*x_last + 2.0*c*x_last+d;
+        tf::Transform pose_last;
+        pose_last.setOrigin(tf::Vector3(x_last, y_last, 0));
+        pose_last = get_tf(x_last, y_last, gradient_last);
+        trajectory_.push_back(pose_last);
+    }
+
+    //将轨迹点反变换到原坐标系
+    //tf::Transform tf_inv=axis_transform_.inverse();
+    for(int i=0;i<trajectory_.size();++i)
+    {
+        trajectory_[i]=Rotate_pose(trajectory_[i],-angle);//trajectory_[i]*tf_inv;
+    }
+    return true;
+}

src/agv_bundle/src/moving_control/FourthTrajectory.h

@@ -0,0 +1,18 @@
+//
+// Created by zx on 2019/8/31.
+//
+
+#ifndef FOURTHTRAJECTORY_H
+#define FOURTHTRAJECTORY_H
+#include "make_trajectory.h"
+
+class FourthTrajectory :public MakeTrajectory
+{
+public:
+    FourthTrajectory();
+    ~FourthTrajectory();
+    virtual bool Make(tf::Transform t0,tf::Transform t1, int extend_num=0);
+};
+
+
+#endif //FOURTHTRAJECTORY_H

src/agv_bundle/src/moving_control/MonitorMPC.cpp

@@ -0,0 +1,245 @@
+#include "MonitorMPC.h"
+#include <cppad/cppad.hpp>
+#include <cppad/ipopt/solve.hpp>
+#include "Eigen/Core"
+#include "Eigen/QR"
+
+using CppAD::AD;
+
+// Set the timestep length and duration
+// Currently tuned to predict 1 second worth
+size_t N = 10;
+
+
+// The solver takes all the state variables and actuator
+// variables in a singular vector. Thus, we should to establish
+// when one variable starts and another ends to make our lifes easier.
+size_t nx = 0;
+size_t ny = nx + N;
+size_t nth = ny + N;
+size_t nv = nth + N;
+size_t nvth = nv + N;
+
+
+class FG_eval {
+public:
+    // Fitted polynomial coefficients
+    Eigen::VectorXd coeffs;
+    double m_ref_v,m_dt;
+    double m_v,m_vth;
+    FG_eval(Eigen::VectorXd coeffs,double v,double vth,double ref_v,double dt) {
+        this->coeffs = coeffs;
+        m_v=v;
+        m_vth=vth;
+        m_ref_v=ref_v;
+        m_dt=dt;
+    }
+
+    typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
+    void operator()(ADvector& fg, const ADvector& vars) {
+        // Implementing MPC below
+        // `fg` a vector of the cost constraints, `vars` is a vector of variable values (state & actuators)
+        // The cost is stored is the first element of `fg`.
+        // Any additions to the cost should be added to `fg[0]`.
+        fg[0] = 0;
+        double dt=m_dt;
+        // Reference State Cost
+        // Below defines the cost related the reference state and
+        // any anything you think may be beneficial.
+
+        // Weights for how "important" each cost is - can be tuned
+        const int y_cost_weight = 1500;
+        const int th_cost_weight = 1000;
+        const int v_cost_weight = 20;
+        const int vth_cost_weight = 100;
+        const int a_cost_weight = 1;
+        const int ath_cost_weight=100;
+
+        // Cost for CTE, psi error and velocity
+        for (int t = 0; t < N; t++) {
+            AD<double> xt=vars[nx+t];
+            AD<double> fx = coeffs[0] + coeffs[1] * xt + coeffs[2] * pow(xt, 2) + coeffs[3] * pow(xt, 3);
+            fg[0] += y_cost_weight * CppAD::pow(vars[ny + t]-fx, 2);
+
+            AD<double> fth = CppAD::atan(coeffs[1] + 2*coeffs[2]*xt + 3*coeffs[3]*pow(xt,2));
+            fg[0] += th_cost_weight * CppAD::pow(vars[nth + t]-fth, 2);
+
+            fg[0]+=v_cost_weight*CppAD::pow(vars[nv+t]-m_ref_v,2);
+
+        }
+
+        // Costs for steering (delta) and acceleration (a)
+        for (int t = 0; t < N-1; t++) {
+            fg[0] += vth_cost_weight * CppAD::pow(vars[nvth + t], 2);
+            fg[0] += a_cost_weight * CppAD::pow(vars[nv + t+1]-vars[nv+t], 2);
+            fg[0] += ath_cost_weight * CppAD::pow(vars[nvth + t+1]-vars[nvth+t], 2);
+        }
+
+        /////////////////////
+        fg[1 + nx] = vars[nx]-vars[nv]*dt;
+        fg[1 + ny] = vars[ny];
+        fg[1 + nth] = vars[nth]-vars[nvth]*dt;
+
+        // The rest of the constraints
+        for (int t = 1; t < N; t++) {
+            // State at time t + 1
+            AD<double> x1 = vars[nx + t];
+            AD<double> y1 = vars[ny + t];
+            AD<double> th1 = vars[nth + t];
+            AD<double> v1 = vars[nv + t];
+            AD<double> vth1 = vars[nvth + t];
+
+            // State at time t
+            AD<double> x0 = vars[nx + t -1];
+            AD<double> y0 = vars[ny + t -1];
+            AD<double> th0 = vars[nth + t-1];
+            AD<double> v0 = vars[nv + t-1];
+            AD<double> vth0 = vars[nvth + t-1];
+
+            // Actuator constraints at time t only
+
+            // Setting up the rest of the model constraints
+            fg[1 + nx + t] = x1 - (x0 + v0 * CppAD::cos(th0) * dt);
+            fg[1 + ny + t] = y1 - (y0 + v0 * CppAD::sin(th0) * dt);
+            fg[1 + nth + t] = th1 - (th0 + vth0 * dt);
+
+        }
+
+        fg[1 + nv]=(vars[nv]-m_v)/dt;
+        fg[1+nvth]=(vars[nvth]-m_vth)/dt;
+        for(int t=1;t<N;++t)
+        {
+            fg[1+nv+t]=(vars[nv+t]-vars[nv+t-1])/dt;
+            fg[1+nvth+t]=(vars[nvth+t]-vars[nvth+t-1])/dt;
+        }
+
+        ////  add
+
+    }
+};
+
+//
+// MPC class definition implementation.
+//
+MonitorMPC::MonitorMPC() {}
+MonitorMPC::~MonitorMPC() {}
+
+bool MonitorMPC::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs, std::vector<double>& out) {
+    bool ok = true;
+    typedef CPPAD_TESTVECTOR(double) Dvector;
+
+    // State vector holds all current values neede for vars below
+    double v = state[0];
+    double vth = state[1];
+    double ref_v = state[2];
+    double max_v=state[3];
+    double dt=state[4];
+
+
+    // Setting the number of model variables (includes both states and inputs).
+    // N * state vector size + (N - 1) * 2 actuators (For steering & acceleration)
+    size_t n_vars = N * 5;
+    // Setting the number of constraints
+    size_t n_constraints = N * 5;
+
+    // Initial value of the independent variables.
+    // SHOULD BE 0 besides initial state.
+    Dvector vars(n_vars);
+    for (int i = 0; i < n_vars; i++) {
+        vars[i] = 0.0;
+    }
+
+    Dvector vars_lowerbound(n_vars);
+    Dvector vars_upperbound(n_vars);
+    // Sets lower and upper limits for variables.
+    // Set all non-actuators upper and lowerlimits
+    // to the max negative and positive values.
+    for (int i = 0; i < n_vars; i++) {
+        vars_lowerbound[i] = -1.0e19;
+        vars_upperbound[i] = 1.0e19;
+    }
+
+    //// limit  v
+    for (int i = nv; i < nv+N; i++) {
+        vars_lowerbound[i] = -max_v;
+        vars_upperbound[i] = max_v;
+    }
+
+    ////limint vth
+    for (int i = nvth; i < nvth+N; i++) {
+        vars_lowerbound[i] = -0.2;
+        vars_upperbound[i] = 0.2;
+    }
+
+    // Lower and upper limits for the constraints
+    // Should be 0 besides initial state.
+    Dvector constraints_lowerbound(n_constraints);
+    Dvector constraints_upperbound(n_constraints);
+    for (int i = 0; i < n_constraints; i++) {
+        constraints_lowerbound[i] = 0;
+        constraints_upperbound[i] = 0;
+    }
+    //// acc v
+    for (int i = nv; i < nv+N; i++) {
+        constraints_lowerbound[i] = -2.0;
+        constraints_upperbound[i] = 2.0;
+    }
+    //// acc vth
+    for (int i = nvth; i < nvth+N; i++) {
+        constraints_lowerbound[i] = -0.5;
+        constraints_upperbound[i] = 0.5;
+    }
+
+
+    // object that computes objective and constraints
+    FG_eval fg_eval(coeffs,v,vth,ref_v,dt);
+
+    //
+    // NOTE: You don't have to worry about these options
+    //
+    // options for IPOPT solver
+    std::string options;
+    // Uncomment this if you'd like more print information
+    options += "Integer print_level  0\n";
+    // NOTE: Setting sparse to true allows the solver to take advantage
+    // of sparse routines, this makes the computation MUCH FASTER. If you
+    // can uncomment 1 of these and see if it makes a difference or not but
+    // if you uncomment both the computation time should go up in orders of
+    // magnitude.
+    options += "Sparse  true        forward\n";
+    options += "Sparse  true        reverse\n";
+    // NOTE: Currently the solver has a maximum time limit of 0.5 seconds.
+    // Change this as you see fit.
+    options += "Numeric max_cpu_time          0.5\n";
+
+    // place to return solution
+    CppAD::ipopt::solve_result<Dvector> solution;
+
+    // solve the problem
+    CppAD::ipopt::solve<Dvector, FG_eval>(
+        options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
+        constraints_upperbound, fg_eval, solution);
+
+    // Check some of the solution values
+    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
+
+    // Cost
+    auto cost = solution.obj_value;
+    //std::cout << "Cost " << cost << std::endl;
+
+    // Return the first actuator values, along with predicted x and y values to plot in the simulator.
+    out.clear();
+    m_trajectory.clear();
+    out.push_back(solution.x[nv]);
+    out.push_back(solution.x[nvth]);
+    for (int i = 0; i < N; ++i) {
+        out.push_back(solution.x[nx + i]);
+        out.push_back(solution.x[ny + i]);
+        tf::Pose pose;
+        pose.setOrigin(tf::Vector3(solution.x[nx+i],solution.x[ny+i],0));
+        m_trajectory.push_back(pose);
+    }
+    //printf(" cost : %.3f\n",cost);
+    return ok;
+
+}

src/agv_bundle/src/moving_control/MonitorMPC.h

@@ -0,0 +1,24 @@
+//
+// Created by zx on 2019/8/28.
+//
+
+#ifndef MONITORMPC_H
+#define MONITORMPC_H
+
+#include <vector>
+#include <Eigen/Core>
+#include <tf/transform_datatypes.h>
+
+class MonitorMPC
+{
+public:
+    MonitorMPC();
+    virtual ~MonitorMPC();
+
+    bool Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs, std::vector<double>& out);
+
+    std::vector<tf::Pose> m_trajectory;
+};
+
+
+#endif //MONITORMPC_H

src/agv_bundle/src/moving_control/make_trajectory.cpp

@@ -0,0 +1,282 @@
+#include "make_trajectory.h"
+#include <limits>
+
+double get_pose_angle_z(tf::Transform pos)
+{
+    double yaw,a,b;
+    pos.getBasis().getEulerZYX(yaw,a,b);
+    return yaw;
+}
+
+double get_gradient_by_tf(tf::Transform pos)
+{
+    double angle=get_pose_angle_z(pos);
+    double gradient=tanf(angle);
+    if(fabs(gradient)>200)
+        return 200.0*(gradient/fabs(gradient));
+    return gradient;
+}
+
+tf::Transform get_tf(double x,double y,double gradient)
+{
+    double angle=0;
+    if(fabs(gradient)>200.0)
+    {
+        angle=M_PI/2.0*(gradient/fabs(gradient));
+    }
+    else
+    {
+        angle=atan(fabs(gradient));
+        angle=angle*(gradient/fabs(gradient));
+    }
+    tf::Matrix3x3 rotation;
+    rotation.setRPY(0,0,angle);
+    tf::Transform tf;
+    tf.setBasis(rotation);
+    tf.setOrigin(tf::Vector3(x,y,0));
+    return tf;
+}
+
+tf::Transform transform_tf(tf::Transform pos,tf::Transform tf)
+{
+    pos=pos*tf;
+    return pos;
+}
+
+tf::Pose Rotate_pose(tf::Pose pose,double angle)
+{
+    //先平移，后旋转
+    double x=pose.getOrigin().getX();
+    double y=pose.getOrigin().getY();
+
+    double cs=cos(angle);
+    double sn=sin(angle);
+    double x1=cs*x+sn*y;
+    double y1=-sn*x+cs*y;
+    tf::Pose newpose;
+
+    tf::Quaternion quat;
+    double theta,a,b;
+    pose.getBasis().getEulerZYX(theta,a,b);
+    quat.setEulerZYX(theta-angle,a,b);
+    newpose.setRotation(quat);
+    newpose.setOrigin(tf::Vector3(x1,y1,0));
+    return newpose;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+MakeTrajectory::MakeTrajectory()
+{
+    left_tf_=tf::Transform(tf::Quaternion(0,0,0));
+    right_tf_=tf::Transform(tf::Quaternion(0,0,0));
+    
+}
+bool MakeTrajectory::TransformTrajectory(tf::Transform tf)
+{
+
+    return true;
+}
+tf::Transform MakeTrajectory::operator[](int n)
+{
+    boost::mutex::scoped_lock lock(mutex_trajectory_);
+    return trajectory_[n];
+
+}
+bool MakeTrajectory::Make(tf::Transform t0,tf::Transform t1, int extend_num)
+{
+    position0_=t0;
+    position1_=t1;
+
+    double angle =gen_axis_angle();
+
+    tf::Transform pose1=Rotate_pose(position0_,angle);
+    tf::Transform pose2=Rotate_pose(position1_,angle);
+    double gradient1=get_gradient_by_tf(pose1);
+    double gradient2=get_gradient_by_tf(pose2);
+    
+    //std::cout<<"grad1 grad2 :"<<gradient1<<" ,"<<gradient2<<std::endl;
+    //三阶多项式差值，生成 曲线
+    double x0=pose1.getOrigin().getX();
+    double y0=pose1.getOrigin().getY();
+    double x1=pose2.getOrigin().getX();
+    double y1=pose2.getOrigin().getY();
+
+    //std::cout<<" x1 ,x2 : "<<x0<<" , "<<x1<<std::endl;
+
+    Eigen::Matrix<double,4,4> A=Eigen::MatrixXd::Zero(4,4);    //系数矩阵A
+    Eigen::Matrix<double,4,1> B=Eigen::MatrixXd::Zero(4,1);    //值矩阵B
+    A(0,0)=pow(x0,3.0);
+    A(0,1)=pow(x0,2.0);
+    A(0,2)=x0;
+    A(0,3)=1.0;
+
+    A(1,0)=pow(x1,3.0);
+    A(1,1)=pow(x1,2.0);
+    A(1,2)=x1;
+    A(1,3)=1.0;
+
+    A(2,0)=3.0*pow(x0,2.0);
+    A(2,1)=2.0*x0;
+    A(2,2)=1.0;
+    
+    A(3,0)=3.0*pow(x1,2.0);
+    A(3,1)=2.0*x1;
+    A(3,2)=1.0;
+
+    B(0,0)=y0;
+    B(1,0)=y1;
+    B(2,0)=gradient1;
+    B(3,0)=gradient2;
+    Eigen::MatrixXd P=A.inverse()*B;
+    line_param_=P;
+
+    double a=P(0,0);
+    double b=P(1,0);
+    double c=P(2,0);
+    double d=P(3,0);
+    //生成离散点,间距 step (m)
+
+    boost::mutex::scoped_lock lock(mutex_trajectory_);
+    trajectory_.clear();
+
+    //计算两轮间距/2
+    double distance=0.5*sqrt(pow(left_tf_.getOrigin().getX()-right_tf_.getOrigin().getX(),2.0)+
+                            pow(left_tf_.getOrigin().getY()-right_tf_.getOrigin().getY(),2.0));
+    distance+=0.00000001;
+
+    double delta=0.05;
+    double x=x0;
+    // added by yct
+    int extend_front_count = 0;
+    int extend_end_count = 0;
+    int sign = 1;
+    //modified by yct
+    while(x<x1 || (extend_num>0 && extend_end_count<=extend_num))
+    {
+        double y=a*pow(x,3.0)+b*pow(x,2.0)+c*x+d;
+        double gradient=3.0*a*pow(x,2.0)+2.0*b*x+c;
+
+        //计算x处的角度变化率
+        double gradtheta=threta_gradient(x);
+        if(fabs(gradtheta)>1.0/distance)
+        {
+            trajectory_.clear();
+            return false;
+        }
+
+        tf::Transform pose;
+        pose.setOrigin(tf::Vector3(x,y,0));
+        pose=get_tf(x,y,gradient);
+
+        // modified by yct
+        // 开头逆向移动x到扩展头，再开始入队.
+        if (extend_num > 0)
+        {
+            if(extend_front_count <= extend_num)
+            {
+                extend_front_count++;
+                sign = -1;
+            }else{
+                sign = 1;
+                trajectory_.push_back(pose);
+                if(x>=x1 && extend_end_count <= extend_num){
+                    extend_end_count++;
+                }
+            }
+        }
+        else{
+            trajectory_.push_back(pose);
+        }
+
+        double delta_x=sqrt(delta*delta/(1.0+gradient*gradient));
+        // modified by yct
+        x+=delta * sign;
+    }
+
+    //最后一点加入轨迹中, modified by yct, 非扩展手动加最后一点
+    if (extend_num <= 0)
+    {
+        double x_last = x1;
+        double y_last = a * pow(x_last, 3.0) + b * pow(x_last, 2.0) + c * x_last + d;
+        double gradient_last = 3.0 * a * pow(x_last, 2.0) + 2.0 * b * x_last + c;
+        tf::Transform pose_last;
+        pose_last.setOrigin(tf::Vector3(x_last, y_last, 0));
+        pose_last = get_tf(x_last, y_last, gradient_last);
+        trajectory_.push_back(pose_last);
+    }
+
+    //将轨迹点反变换到原坐标系
+    //tf::Transform tf_inv=axis_transform_.inverse();
+    for(int i=0;i<trajectory_.size();++i)
+    {
+        trajectory_[i]=Rotate_pose(trajectory_[i],-angle);//trajectory_[i]*tf_inv;
+    }
+    return true;
+}
+double MakeTrajectory::gen_axis_angle()
+{
+
+    double x=position1_.getOrigin().getX()-position0_.getOrigin().getX();
+    double y=position1_.getOrigin().getY()-position0_.getOrigin().getY();
+    tf::Vector3 var(x,y,0);
+
+    double angle=atan2(y,x);
+
+    //std::cout<<" var.angle : "<<angle*180.0/M_PI<<std::endl;
+    return angle;
+
+}
+
+void MakeTrajectory::SetLRTransform(tf::Transform left,tf::Transform right)
+{
+    left_tf_=left;
+    right_tf_=right;
+}
+
+bool MakeTrajectory::IsPoseInTrajectory(tf::Transform pose)
+{
+    boost::mutex::scoped_lock lock(mutex_trajectory_);
+    if(trajectory_.size()==0)
+        return false;
+
+    double angle0=get_pose_angle_z(pose);
+    double min_distance=std::numeric_limits<double>::max();
+    tf::Transform min_point;
+    for(int i=0;i<trajectory_.size();++i)
+    {
+        tf::Transform tf=trajectory_[i];
+        double distance=sqrt(pow(pose.getOrigin().getX()-tf.getOrigin().getX(),2.0)+
+                            pow(pose.getOrigin().getY()-tf.getOrigin().getY(),2.0));
+        if(distance < min_distance)
+        {
+            min_distance=distance;
+            min_point=tf;
+        }
+    }
+    if(min_distance==std::numeric_limits<double>::max())
+        return false;
+    double angle1=get_pose_angle_z(min_point);
+    double angle_distance=fabs(angle1-angle0);
+    
+    if(min_distance>0.05 || angle_distance*180.0/M_PI>10.0)
+    {
+        std::cout<<"distance :"<<min_distance<<"   angle distance : "<<angle_distance*180.0/M_PI<<std::endl;
+        return false;
+    }
+
+    return true;
+}
+
+double MakeTrajectory::threta_gradient(double x)
+{
+    double a=line_param_(0,0);
+    double b=line_param_(1,0);
+    double c=line_param_(2,0);
+    double d=line_param_(3,0);
+    double S=1+pow(3*a*pow(x,2.0)+2*b*x+c,2.0);
+    double s=6*a*x+2*b;
+    return s/S;
+}

src/agv_bundle/src/moving_control/make_trajectory.h

@@ -0,0 +1,47 @@
+#ifndef __GEN_TRAJECTORY__
+#define __GEN_TRAJECTORY__
+/*
+ * 三次曲线获取轨迹，两点坐标+两点梯度 求曲线参数
+ */
+#include <tf/tf.h>
+#include "tf2_msgs/TFMessage.h"
+#include <boost/thread.hpp>
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <vector>
+
+double get_pose_angle_z(tf::Transform pos);
+double get_gradient_by_tf(tf::Transform pos);
+tf::Transform get_tf(double x,double y,double gradient);
+tf::Transform transform_tf(tf::Transform pos,tf::Transform tf);
+tf::Pose Rotate_pose(tf::Pose pose,double angle);
+
+
+class MakeTrajectory
+{
+public:
+    MakeTrajectory();
+    bool TransformTrajectory(tf::Transform tf);
+    void SetLRTransform(tf::Transform letf,tf::Transform right);
+    tf::Transform operator[](int n);
+    int Size(){return trajectory_.size();}
+    virtual bool Make(tf::Transform t0,tf::Transform t1, int extend_num=0);
+    bool IsPoseInTrajectory(tf::Transform pose);
+protected:
+    //计算当前x点的 角度变化率
+    double threta_gradient(double x);
+    double  gen_axis_angle();
+public:
+    std::vector<tf::Transform>      trajectory_;                //雷达轨迹
+protected:
+    tf::Transform                   position0_;
+    tf::Transform                   position1_;
+
+    tf::Transform                   left_tf_;
+    tf::Transform                   right_tf_;
+    Eigen::MatrixXd                 line_param_;                //曲线参数
+
+    boost::mutex	                mutex_trajectory_;
+};
+
+#endif

src/agv_bundle/src/moving_control/mpc_tools.cpp

@@ -0,0 +1,78 @@
+#include  "mpc_tools.h"
+
+constexpr double pi() { return M_PI; }
+double deg2rad(double x) { return x * pi() / 180; }
+double rad2deg(double x) { return x * 180 / pi(); }
+
+
+mpc_tools::mpc_tools()
+{
+}
+
+mpc_tools::~mpc_tools()
+{
+}
+
+//获取小车坐标系中，x前进方向 num 个点，用于拟合
+bool mpc_tools::filterPath(tf::Pose pose,std::vector<tf::Pose> poses, std::vector<tf::Pose>& out,
+    int numOfPoints,bool direct){
+  if(poses.size() < numOfPoints)
+    return false;
+
+ out.clear();
+  // 平移加反向旋转到小车坐标系
+    double theta,a,b;
+    pose.getBasis().getEulerZYX(theta,a,b);
+    if(direct==true) theta+=M_PI;
+  for (int i = 0; i < poses.size(); i++)
+  {
+
+    double x = poses[i].getOrigin().getX()-pose.getOrigin().getX();
+    double y = poses[i].getOrigin().getY()-pose.getOrigin().getY();
+    double trans_x = x * cos(-theta) - y * sin(-theta);
+    double trans_y = x * sin(-theta) + y * cos(-theta);
+    if (trans_x>=0  && out.size() < numOfPoints)
+    {
+        tf::Pose point;
+        float nx=trans_x * cos(theta) - trans_y * sin(theta);
+        float ny=trans_x*sin(theta)+trans_y*cos(theta);
+        point.setOrigin(tf::Vector3(nx+pose.getOrigin().getX(),
+            ny+pose.getOrigin().getY(),0));
+        out.push_back(point);
+
+    }
+  }
+  return true;
+
+}
+
+// 求y
+double mpc_tools::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;
+}
+
+// 求参数
+Eigen::VectorXd mpc_tools::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;
+}
+

src/agv_bundle/src/moving_control/mpc_tools.h

@@ -0,0 +1,29 @@
+#ifndef MPC_TOOLS_HH
+#define MPC_TOOLS_HH 
+
+#include <vector>
+#include <Eigen/Core>
+#include <Eigen/QR>
+#include <tf/transform_datatypes.h>
+
+
+// For converting back and forth between radians and degrees.
+
+
+class mpc_tools{
+
+public:
+    mpc_tools();
+    ~mpc_tools();
+
+    Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order);
+
+    double polyeval(Eigen::VectorXd coeffs, double x);
+
+    bool filterPath(tf::Pose pose,std::vector<tf::Pose> poses, std::vector<tf::Pose>& out,
+        int numOfPoints,bool derect=false);
+
+
+};
+
+#endif /* MPC_TOOLS_HH */

src/agv_bundle/src/moving_control_node.cpp

@@ -0,0 +1,337 @@
+/*
+ * @Description: 运动控制节点，接收goal，odom，tf生成并发布路径，优化并发布cmd_vel
+ * @Author: yct
+ * @Date: 2020-06-11 17:27:41
+ * @LastEditTime: 2020-06-11 17:41:22
+ * @LastEditors: yct
+ */ 
+#include <iostream>
+#include <tf/transform_datatypes.h>
+#include <nav_msgs/Odometry.h>
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+#include <nav_msgs/Path.h>
+#include <string>
+#include <chrono>
+#include <turtlesim/Pose.h>
+
+#include "moving_control/MonitorMPC.h"
+#include "moving_control/make_trajectory.h"
+#include "moving_control/FourthTrajectory.h"
+#include "moving_control/mpc_tools.h"
+#include <tf/transform_broadcaster.h>
+
+geometry_msgs::Twist g_cmd_vel;
+ros::Publisher g_vel_pub, g_vel_pub1, g_pose_pub1;
+ros::Publisher g_local_plan_pub,g_global_plan_pub,g_fit_plan_pub;
+tf::Pose g_cur_pose,g_goal;
+bool g_goal_valid=false;
+
+FourthTrajectory maker;
+mpc_tools mpc_tool;
+double g_max_vx;
+
+void MPC();
+
+void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
+{
+    printf("new goal recieve ...  \n");
+    double x=msg->pose.position.x;
+    double y=msg->pose.position.y;
+    double z=msg->pose.position.z;
+    double qx=msg->pose.orientation.x;
+    double qy=msg->pose.orientation.y;
+    double qz=msg->pose.orientation.z;
+    double qw=msg->pose.orientation.w;
+    g_goal.setOrigin(tf::Vector3(x,y,z));
+    tf::Quaternion quat(qx,qy,qz,qw);
+    g_goal.setRotation(quat);
+
+    ///更新轨迹
+
+    if(maker.Make(g_cur_pose,g_goal,20)) {
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(maker.Size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for (unsigned int i = 0; i < maker.Size(); i++) {
+            gui_path.poses[i].pose.position.x = maker[i].getOrigin().getX();
+            gui_path.poses[i].pose.position.y = maker[i].getOrigin().getY();
+            tf::Quaternion quat=maker[i].getRotation();
+
+            gui_path.poses[i].pose.orientation.x = quat.x();
+            gui_path.poses[i].pose.orientation.y = quat.y();
+            gui_path.poses[i].pose.orientation.z = quat.z();
+            gui_path.poses[i].pose.orientation.w = quat.w();
+        }
+
+        g_global_plan_pub.publish(gui_path);
+        g_goal_valid=true;
+
+    }
+
+}
+
+// 一号车位姿回调
+void poseCallback1(const turtlesim::Pose::ConstPtr& msg)
+{
+    double theta=msg->theta;
+    tf::Quaternion q;
+    q.setRPY(0,0,theta);
+    g_cur_pose.setRotation(q);
+    g_cur_pose.setOrigin(tf::Vector3(msg->x,msg->y,0));
+
+    g_cmd_vel.linear.x=msg->linear_velocity;
+    g_cmd_vel.angular.z=msg->angular_velocity;
+
+    geometry_msgs::PoseStamped pose;
+    pose.header.frame_id="map";
+    pose.header.stamp=ros::Time::now();
+    pose.pose.position.x=g_cur_pose.getOrigin().getX();
+    pose.pose.position.y=g_cur_pose.getOrigin().getY();
+    pose.pose.position.z=0;
+    pose.pose.orientation.x=q.x();
+    pose.pose.orientation.y=q.y();
+    pose.pose.orientation.z=q.z();
+    pose.pose.orientation.w=q.w();
+    g_pose_pub1.publish(pose);
+
+    MPC();
+
+}
+
+void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
+{
+    ROS_INFO_ONCE("odom received!");
+    static tf::TransformListener tf_listen(ros::Duration(10));
+
+    g_cmd_vel.linear.x = msg->twist.twist.linear.x;
+    g_cmd_vel.angular.z = msg->twist.twist.angular.z;
+    std::string frame_id = msg->child_frame_id;
+    std::string tf_error;
+    tf::StampedTransform transform;
+    try{
+        tf_listen.lookupTransform("/map", "/base_link",ros::Time(0), transform);
+        g_cur_pose=transform;
+    }
+    catch (tf::TransformException &ex) {
+        if(g_goal_valid)
+            ROS_ERROR(" tf listen error : %s",ex.what());
+        return;
+    }
+    MPC();
+}
+
+void MPC()
+{
+    auto start = std::chrono::system_clock::now();
+    static double dt=0.05;
+    int fitNum=20;
+    if(g_goal_valid==false || maker.Size()<fitNum ) return ;
+//////判断是否到达目标点
+    double x = g_cur_pose.getOrigin().getX();
+    double y = g_cur_pose.getOrigin().getY();
+    double gx = g_goal.getOrigin().getX();
+    double gy = g_goal.getOrigin().getY();
+    double yaw;
+    double theta,a,b;
+    g_cur_pose.getBasis().getEulerZYX(theta,a,b);
+    g_goal.getBasis().getEulerZYX(yaw, a, b);
+
+    double dx=g_goal.getOrigin().getX()-g_cur_pose.getOrigin().getX();
+    double dy=g_goal.getOrigin().getY()-g_cur_pose.getOrigin().getY();
+    double dis_th = fabs(yaw - theta);
+    if (fabs(dx) < 0.05 && fabs(dy) < 0.03 && dis_th < 0.02 && fabs(g_cmd_vel.linear.x) < 0.05 && fabs(g_cmd_vel.angular.z) < 0.05)
+    {
+        geometry_msgs::Twist cmd_vel;
+        cmd_vel.linear.x = 0;
+        cmd_vel.linear.y = 0.0;
+        cmd_vel.angular.z = 0;
+        g_vel_pub.publish(cmd_vel);
+        g_vel_pub1.publish(cmd_vel);
+
+        g_goal_valid = false;
+        std::cout<<"   到达目标点 !!!!"<<std::endl;
+        return;
+    }
+
+    //////////////////////////////////////////////////   导航 部分  ///////////////////////////////////
+    tf::Vector3 length(dx,dy,0);
+
+    double direct_x = dx * cos(-theta) - dy * sin(-theta);
+
+    double ref_v=length.length()/(g_max_vx*1.5);
+
+    ref_v=std::max(ref_v,0.02);
+    ref_v=std::min(ref_v,g_max_vx);
+
+    if (direct_x<0) ref_v=-ref_v;
+
+    ///// 选点
+    std::vector<tf::Pose> poses;
+    if(!mpc_tool.filterPath(g_cur_pose, maker.trajectory_,poses,fitNum,direct_x<0))
+    {
+        ROS_ERROR(" fit error ...");
+        return ;
+    }
+    else
+    {
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(poses.size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for(unsigned int i=0; i < poses.size(); i++)
+        {
+            float x=poses[i].getOrigin().getX();
+            float y=poses[i].getOrigin().getY();
+
+            gui_path.poses[i].pose.position.x=x;
+            gui_path.poses[i].pose.position.y=y;
+            tf::Quaternion quat=poses[i].getRotation();
+            gui_path.poses[i].pose.orientation.x=quat.x();
+            gui_path.poses[i].pose.orientation.y=quat.y();
+            gui_path.poses[i].pose.orientation.z=quat.z();
+            gui_path.poses[i].pose.orientation.w=quat.w();
+        }
+        //std::cout<<"publish "<<mpc.m_trajectory.size()<<std::endl;
+        g_fit_plan_pub.publish(gui_path);
+
+    }
+
+    //////   fit   select points
+    Eigen::VectorXd ptsx_car(poses.size());
+    Eigen::VectorXd ptsy_car(poses.size());
+
+    for (int i = 0; i < poses.size(); i++)
+    {
+        double x = poses[i].getOrigin().getX() - g_cur_pose.getOrigin().getX();
+        double y = poses[i].getOrigin().getY() - g_cur_pose.getOrigin().getY();
+        ptsx_car[i] = x * cos(-theta) - y * sin(-theta);
+        ptsy_car[i] = x * sin(-theta) + y * cos(-theta);
+    }
+
+    Eigen::VectorXd coeffs=mpc_tool.polyfit(ptsx_car,ptsy_car,3);
+
+    Eigen::VectorXd state(5);
+    state << g_cmd_vel.linear.x, g_cmd_vel.angular.z, ref_v, g_max_vx,dt;
+    MonitorMPC mpc;
+    std::vector<double> out;
+
+    if(mpc.Solve(state,coeffs,out))
+    {
+        geometry_msgs::Twist cmd_vel;
+        cmd_vel.linear.x = out[0];
+        cmd_vel.linear.y = 0.0;
+        cmd_vel.angular.z = out[1];
+        g_vel_pub.publish(cmd_vel);
+        g_vel_pub1.publish(cmd_vel);
+
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(mpc.m_trajectory.size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for (unsigned int i = 0; i < mpc.m_trajectory.size(); i++) {
+            float x = mpc.m_trajectory[i].getOrigin().getX();
+            float y = mpc.m_trajectory[i].getOrigin().getY();
+            float rx = x * cos(theta) - y * sin(theta) + g_cur_pose.getOrigin().getX();
+            float ry = x * sin(theta) + y * cos(theta) + g_cur_pose.getOrigin().getY();
+            gui_path.poses[i].pose.position.x = rx;
+            gui_path.poses[i].pose.position.y = ry;
+            tf::Quaternion quat = mpc.m_trajectory[i].getRotation();
+            gui_path.poses[i].pose.orientation.x = quat.x();
+            gui_path.poses[i].pose.orientation.y = quat.y();
+            gui_path.poses[i].pose.orientation.z = quat.z();
+            gui_path.poses[i].pose.orientation.w = quat.w();
+        }
+        //std::cout<<"publish "<<mpc.m_trajectory.size()<<std::endl;
+        g_local_plan_pub.publish(gui_path);
+
+        auto end = std::chrono::system_clock::now();
+        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+        double time=double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
+        printf(" ref:%.3f, best_v:%.3f, best_th:%.3f, dt:%.3f\n",ref_v,out[0],out[1],time);
+        double sleep_t=(dt-time)*1000.0;
+        if(sleep_t>0)
+            usleep(1000*int(sleep_t));
+
+
+    }
+}
+
+void updateGoal(const geometry_msgs::PoseStamped::ConstPtr& msg)
+{
+    printf("update goal recieve ...  \n");
+    double x=g_cur_pose.getOrigin().getX();
+    double y=g_cur_pose.getOrigin().getY();
+    double z=g_cur_pose.getOrigin().getZ();
+    double qx=g_cur_pose.getRotation().getX();
+    double qy=g_cur_pose.getRotation().getY();
+    double qz=g_cur_pose.getRotation().getZ();
+    double qw=g_cur_pose.getRotation().getW();
+    g_goal.setOrigin(tf::Vector3(x+msg->pose.position.x,y,z));
+    tf::Quaternion quat(qx,qy,qz,qw);
+    g_goal.setRotation(quat);
+
+    ///更新轨迹
+    if(maker.Make(g_cur_pose,g_goal,10)) {
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(maker.Size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for (unsigned int i = 0; i < maker.Size(); i++) {
+            gui_path.poses[i].pose.position.x = maker[i].getOrigin().getX();
+            gui_path.poses[i].pose.position.y = maker[i].getOrigin().getY();
+            tf::Quaternion quat=maker[i].getRotation();
+
+            gui_path.poses[i].pose.orientation.x = quat.x();
+            gui_path.poses[i].pose.orientation.y = quat.y();
+            gui_path.poses[i].pose.orientation.z = quat.z();
+            gui_path.poses[i].pose.orientation.w = quat.w();
+        }
+
+        g_global_plan_pub.publish(gui_path);
+        g_goal_valid=true;
+    }
+}
+int main(int argc ,char* argv[])
+{
+    std::cout<<"  input max vx : ";
+    std::cin>>g_max_vx;
+    std::cout<<std::endl;
+
+    ros::init(argc, argv, "moving_controller");
+
+    ros::NodeHandle gn;
+
+    ros::Subscriber sub = gn.subscribe("/move_base_simple/goal",1,goalCallback);//move_base_simple
+    ros::Subscriber sub0 = gn.subscribe("/goal",1,updateGoal);
+
+    ros::Subscriber odom_sub_ = gn.subscribe<nav_msgs::Odometry>( "/odom", 1, odomCallback);
+    ros::Subscriber pose_sub1_ = gn.subscribe<turtlesim::Pose>( "/turtlesim1/turtle1/pose", 1, poseCallback1);
+
+    g_pose_pub1=gn.advertise<geometry_msgs::PoseStamped>("/turtlePose1", 1);
+
+    g_vel_pub = gn.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+    g_vel_pub1 = gn.advertise<geometry_msgs::Twist>("/turtlesim1/turtle1/cmd_vel", 1);
+    g_global_plan_pub = gn.advertise<nav_msgs::Path>("global_plan",1, true);
+    g_local_plan_pub = gn.advertise<nav_msgs::Path>("local_plan", 1);
+    g_fit_plan_pub = gn.advertise<nav_msgs::Path>("fit_plan", 1);
+
+    ros::spin();
+
+    geometry_msgs::Twist cmd_vel;
+    cmd_vel.linear.x = 0;
+    cmd_vel.linear.y = 0.0;
+    cmd_vel.angular.z = 0;
+    g_vel_pub.publish(cmd_vel);
+
+    return 0;
+}

src/agv_bundle/src/odom/Odom.cpp

@@ -0,0 +1,110 @@
+//
+// Created by zx on 2020/4/28.
+//
+
+#include "Odom.h"
+using namespace agv;
+
+// 无参构造
+Odom::Odom()
+{
+    std::cout<<"odom 构造"<<std::endl;
+    mp_cifx_handle = Cifx_communicator::get_instance();
+    mb_initialized = false;
+    m_odom_callback = nullptr;
+    mp_odom_callback_owner = nullptr;
+}
+
+// 析构
+Odom::~Odom()
+{
+    std::cout<<"odom 析构"<<std::endl;
+    if(mp_cifx_handle != nullptr){
+        Cifx_communicator::release_instance();
+    }
+    if(m_odom_callback!=nullptr)
+    {
+        m_odom_callback = nullptr;
+    }
+    if(mp_odom_callback_owner != nullptr)
+    {
+        mp_odom_callback_owner = nullptr;
+    }
+}
+
+// 初始化函数
+Error_manager Odom::init()
+{
+    if (mp_cifx_handle == nullptr)
+        return POINTER_IS_NULL;
+    // 设置cifx读消息回调
+    Error_manager ec = mp_cifx_handle->set_read_data_callback(Odom::get_cifx_data_callback, this);
+    if(ec != SUCCESS)
+        return ec;
+    // cifx通信卡初始化
+    ec = mp_cifx_handle->cifx_init();
+    if(ec != SUCCESS)
+        return ec;
+    mb_initialized = true;
+    return SUCCESS;
+}
+
+// 获取初始化状态
+bool Odom::is_initialized()
+{
+    return mb_initialized;
+}
+
+// 设置通信卡读数据回调
+Error_manager Odom::set_driver_data_callback(ODOM_CALLBACK func, void *pOwner)
+{
+    m_odom_callback = func;
+    mp_odom_callback_owner = pOwner;
+    return SUCCESS;
+}
+
+// 向通信卡写入电机驱动器数据
+Error_manager Odom::write_driver_data(Driver_data &data)
+{
+    if(mp_cifx_handle == nullptr)
+        return POINTER_IS_NULL;
+    char t_write_data[WHEEL_OUTPUT_AREA_SIZE];
+    memset(t_write_data, 0, WHEEL_OUTPUT_AREA_SIZE);
+    for (size_t i = 0; i < WHEEL_OUTPUT_AREA_SIZE; i+=2)
+    {
+        int16_t t_wheel_data = 0;
+        if((i / 2) < MOTOR_NUM){
+            t_wheel_data = static_cast<int16_t>(data.front_motor_data[i/2] * 1000.0);
+        }else{
+            t_wheel_data = static_cast<int16_t>(data.rear_motor_data[i/2-2] * 1000.0);
+        }
+        // 字节序倒序
+        // t_wheel_data = (t_wheel_data << 8 || t_wheel_data >> 8);
+        memcpy(t_write_data+i, &t_wheel_data, sizeof(int16_t));
+    }
+    return mp_cifx_handle->cifx_write(WHEEL_OUTPUT_AREA, 0, WHEEL_OUTPUT_AREA_SIZE, t_write_data);
+}
+
+
+
+// 向通信卡读数据回调
+void Odom::get_cifx_data_callback(Cifx_communicator::AREA_DATA_BLOCK &read_data, void *pOwner)
+{
+    // std::cout<<"odom data callback"<<std::endl;
+    if(pOwner == nullptr)
+        return;
+    Odom* odom = reinterpret_cast<Odom*>(pOwner);
+    if(odom->m_odom_callback != nullptr && odom->mp_odom_callback_owner != nullptr){
+        Driver_data t_driver_data;
+        for (size_t i = 0; i < WHEEL_OUTPUT_AREA_SIZE; i+=2)
+        {
+            int16_t t_wheel_data = *(reinterpret_cast<uint16_t*>(read_data.data+i));
+            if((i / 2) < MOTOR_NUM){
+                t_driver_data.front_motor_data[i/2] = t_wheel_data / 1000.0;
+            }else{
+                t_driver_data.rear_motor_data[i/2 - 2] = t_wheel_data / 1000.0;
+            }
+        }
+        odom->m_odom_callback(t_driver_data, odom->mp_odom_callback_owner);
+    }
+}

src/agv_bundle/src/odom/Odom.h

@@ -0,0 +1,43 @@
+//
+// Created by zx on 2020/4/28.
+//
+
+#ifndef AGV_ODOM_H
+#define AGV_ODOM_H
+#include <iostream>
+#include "../common/dtype.h"
+#include "../error_code/error_code.h"
+#include "cifx_communicator.h"
+
+namespace agv
+{
+    typedef void (*ODOM_CALLBACK)(Driver_data& data,void* pOwner);
+    
+    class Odom {
+    public:
+        // 无参构造
+        Odom();
+        // 析构
+        ~Odom();
+        // 初始化函数
+        Error_manager init();
+        // 设置通信卡读数据回调
+        Error_manager set_driver_data_callback(ODOM_CALLBACK func,void* pOwner);
+        // 向通信卡写入电机驱动器数据
+        Error_manager write_driver_data(Driver_data& data);
+        // 获取初始化状态
+        bool is_initialized();
+    private:
+        // 向通信卡读数据回调
+        static void get_cifx_data_callback(Cifx_communicator::AREA_DATA_BLOCK& read_data, void* pOwner);
+
+    private:
+        Cifx_communicator* mp_cifx_handle; // cifx 通信卡句柄
+        ODOM_CALLBACK m_odom_callback; // odom 获取新消息回调
+        void* mp_odom_callback_owner; // odom 回调拥有者
+        bool mb_initialized; // 初始化状态
+
+    };
+
+}
+#endif //AGV_ODOM_H

src/agv_bundle/src/odom/cifx_communicator.cpp

@@ -0,0 +1,365 @@
+/*
+ * @Description: cifx RE 50 PNS slave, communicate with PLC through Profinet IO
+ * @Author: yct
+ * @Date: 2020-04-26 13:39:47
+ * @LastEditTime: 2020-04-28 18:01:51
+ * @LastEditors: yct
+ */
+#include "cifx_communicator.h"
+using namespace agv;
+
+Cifx_communicator *Cifx_communicator::g_cifx_ins = nullptr;
+std::mutex Cifx_communicator::g_cifx_access_mutex;
+
+/*****************************************************************************/
+/*! Displays cifX error
+*   \param lError     Error code                                             */
+/*****************************************************************************/
+std::string show_error(int32_t lError)
+{
+    if (lError != CIFX_NO_ERROR)
+    {
+        char szError[1024] = {0};
+        char buf[1152] = {0};
+        xDriverGetErrorDescription(lError, szError, sizeof(szError));
+        sprintf(buf, "Error: 0x%X, <%s>\n", (unsigned int)lError, szError);
+        return std::string(buf);
+    }
+    return std::string("No Error\n");
+}
+
+// 获取通信卡实例
+Cifx_communicator *Cifx_communicator::get_instance()
+{
+    if (g_cifx_ins == nullptr)
+    {
+        std::lock_guard<std::mutex> lck(g_cifx_access_mutex);
+        if (g_cifx_ins == nullptr)
+        {
+            g_cifx_ins = new Cifx_communicator();
+        }
+    }
+    return g_cifx_ins;
+}
+
+// 释放单例
+void Cifx_communicator::release_instance()
+{
+    std::lock_guard<std::mutex> lck(g_cifx_access_mutex);
+    if(g_cifx_ins != nullptr){
+        std::cout<<"cifx 释放"<<std::endl;
+        delete g_cifx_ins;
+        g_cifx_ins = nullptr;
+    }
+}
+
+// 初始化通信卡
+Error_manager Cifx_communicator::cifx_init()
+{
+    m_cifx_state = CIFX_INSTANCE_CREATED;
+    // 清除数据
+    while (m_write_cmd_queue.size() != 0)
+        m_write_cmd_queue.pop();
+    m_cifx_read_data_buffer.clear();
+    // 初始化读写数据块
+    m_cifx_read_data_buffer.push_back(AREA_DATA_BLOCK(WHEEL_INPUT_AREA));
+
+    // 初始化驱动，linux下特有
+    int32_t lRet = cifXDriverInit(&m_init_params);
+    std::string error_str = show_error(lRet);
+    if (lRet != CIFX_NO_ERROR)
+    {
+        return Error_manager(CIFX_DRIVER_INIT_ERROR, Error_level::MAJOR_ERROR, error_str.c_str(), error_str.length());
+    }
+    m_cifx_state = CIFX_DRIVER_INITAILIZED;
+
+    // 开启驱动
+    lRet = xDriverOpen(&m_cifx_driver_handle);
+    error_str = show_error(lRet);
+    if (lRet != CIFX_NO_ERROR)
+    {
+        return Error_manager(CIFX_DRIVER_OPEN_ERROR, Error_level::MAJOR_ERROR, error_str.c_str(), error_str.length());
+    }
+    m_cifx_state = CIFX_DRIVER_OPENED;
+    usleep(1000 * 40); // 驱动开启后需要约40ms的等待时间
+
+    // 开启通道
+    lRet = xChannelOpen(NULL, CIFX_DEV, 0, &m_cifx_driver_handle);
+    error_str = show_error(lRet);
+    if (lRet != CIFX_NO_ERROR)
+    {
+        return Error_manager(CIFX_CHANNEL_OPEN_ERROR, Error_level::MAJOR_ERROR, error_str.c_str(), error_str.length());
+    }
+    m_cifx_state = CIFX_CHANNEL_OPENED;
+
+    // 开启通信总线
+    unsigned long ulState;
+    lRet = xChannelBusState(m_cifx_driver_handle, CIFX_BUS_STATE_ON, (TLR_UINT32 *)&ulState, 10000);
+    error_str = show_error(lRet);
+    if (lRet != CIFX_NO_ERROR)
+    {
+        return Error_manager(CIFX_BUS_OPEN_ERROR, Error_level::MAJOR_ERROR, error_str.c_str(), error_str.length());
+    }
+    m_cifx_state = CIFX_BUS_IS_ON;
+
+    m_data_exchange_thread = new std::thread(&Cifx_communicator::data_exchange, g_cifx_ins);
+    m_data_exchange_thread->detach();
+    mb_initialized = true;
+    return SUCCESS;
+}
+
+// 通过cifx将数据从PLC读入
+Error_manager Cifx_communicator::cifx_read(uint32_t area_number, uint32_t offset, uint32_t data_length, char *data)
+{
+    if (data_length + offset > MAX_DATA_SIZE)
+        return PARAMETER_ERROR;
+    // std::lock_guard<std::mutex> lck(m_cifx_read_mutex);
+    if (m_read_state.state != SUCCESS)
+        return CIFX_READ_FAILED;
+    if (std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - m_read_state.update_time).count() > 20000)
+        return CIFX_READ_TIMEOUT;
+    bool read_success = false;
+    for (size_t i = 0; i < m_cifx_read_data_buffer.size(); i++)
+    {
+        if (m_cifx_read_data_buffer[i].area_number == area_number)
+        {
+            memcpy(data, m_cifx_read_data_buffer[i].data + offset, data_length);
+            read_success = true;
+            break;
+        }
+    }
+    if (!read_success)
+    {
+        return PARAMETER_ERROR;
+    }
+    else
+    {
+        return SUCCESS;
+    }
+}
+
+// 通过cifx将数据写入PLC
+Error_manager Cifx_communicator::cifx_write(uint32_t area_number, uint32_t offset, uint32_t data_length, char *data)
+{
+    // std::lock_guard<std::mutex> lck(m_cifx_write_mutex);
+    if (m_write_state.state != SUCCESS)
+        return CIFX_WRITE_FAILED;
+    // if (std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - m_write_state.update_time).count() > 20000)
+    //     return CIFX_WRITE_TIMEOUT;
+    m_write_cmd_queue.push(WRITE_CMD_DATA_BLOCK(area_number, offset, data_length, data));
+    // std::cout<<"write queue size: "<<m_write_cmd_queue.size()<<std::endl;
+    return SUCCESS;
+}
+
+// 获取初始化状态
+bool Cifx_communicator::is_initialized()
+{
+    return mb_initialized;
+}
+
+// 无参构造函数
+Cifx_communicator::Cifx_communicator()
+{
+    m_read_callback = nullptr;
+    mp_callback_owner = nullptr;
+    m_cifx_state = CIFX_UNINITIALIZED;
+    mb_exit = false;
+    mb_thread_exit = false;
+    mb_initialized = false;
+    m_data_exchange_thread = nullptr;
+    m_cifx_driver_handle = nullptr;
+    m_init_params.init_options = CIFX_DRIVER_INIT_AUTOSCAN;
+    m_init_params.iCardNumber = 0;
+    m_init_params.fEnableCardLocking = 0;
+    m_init_params.base_dir = NULL;
+    m_init_params.poll_interval = 0;
+    m_init_params.poll_StackSize = 0; /* set to 0 to use default */
+    m_init_params.trace_level = 255;
+    m_init_params.user_card_cnt = 0;
+    m_init_params.user_cards = NULL;
+    m_init_params.poll_priority = 0;
+}
+
+// 析构函数
+Cifx_communicator::~Cifx_communicator()
+{
+    mb_exit = true;
+    unsigned long ulState;
+    while(m_data_exchange_thread != nullptr && !mb_thread_exit){
+        // std::cout << "等待线程退出" << std::endl;
+        usleep(1);
+    }
+    std::cout << "析构线程" << std::endl;
+    if (m_data_exchange_thread != nullptr)
+    {
+        if (m_data_exchange_thread->joinable())
+        {
+            m_data_exchange_thread->join();
+        }
+        delete m_data_exchange_thread;
+        m_data_exchange_thread = nullptr;
+    }
+    std::cout << "释放SDK" << std::endl;
+    // 顺序释放cifx通信卡SDK
+    if (m_cifx_state == CIFX_BUS_IS_ON)
+        xChannelBusState(m_cifx_driver_handle, CIFX_BUS_STATE_OFF, (TLR_UINT32 *)&ulState, 10000);
+    if (m_cifx_state == CIFX_CHANNEL_OPENED)
+        xChannelClose(m_cifx_driver_handle);
+    if (m_cifx_state == CIFX_DRIVER_OPENED)
+        xDriverClose(m_cifx_driver_handle);
+    if (m_cifx_state == CIFX_DRIVER_INITAILIZED)
+        cifXDriverDeinit();
+}
+
+// 设置读数据回调，获取到不同数据时激活
+Error_manager Cifx_communicator::set_read_data_callback(CIFX_READ_CALLBACK callback, void* pOwner)
+{
+    m_read_callback = callback;
+    mp_callback_owner = pOwner;
+    return SUCCESS;
+}
+
+// 读写线程函数
+void Cifx_communicator::data_exchange(Cifx_communicator *cifx_handle)
+{
+    std::cout << "线程启动" << std::endl;
+    if (cifx_handle == nullptr)
+    {
+        std::cout << "空指针" << std::endl;
+        return;
+    }
+    if (!cifx_handle->is_initialized())
+    {
+        std::cout << "未初始化" << std::endl;
+        return;
+    }
+    while (!cifx_handle->mb_exit)
+    {
+        // 连接状态正常
+        if (cifx_handle->m_cifx_state == CIFX_BUS_IS_ON)
+        {
+            // std::cout << " 000000 " << std::endl;
+            // 读取PLC数据
+            // cifx_handle->m_cifx_read_mutex.lock();
+            for (size_t i = 0; i < cifx_handle->m_cifx_read_data_buffer.size(); i++)
+            {
+                if (cifx_handle->m_cifx_read_data_buffer[i].area_number == WHEEL_INPUT_AREA)
+                {
+                    // std::cout << " 000111 " << std::endl;
+                    char t_read_data[WHEEL_INPUT_AREA_SIZE]={0};
+                    int32_t lRet = xChannelIORead(cifx_handle->m_cifx_driver_handle,
+                                                  WHEEL_INPUT_AREA, 0, WHEEL_INPUT_AREA_SIZE, t_read_data, 10);
+                    // std::cout << " 000222 " << std::endl;
+                    if (lRet != CIFX_NO_ERROR)
+                    {
+                        if(cifx_handle->m_read_state.state != CIFX_READ_FAILED)
+                            std::cout << "cifx read failed" << std::endl;
+                        cifx_handle->m_read_state.state = CIFX_READ_FAILED;
+                        cifx_handle->m_read_state.update_time = std::chrono::steady_clock::now();
+                    }
+                    else
+                    {
+                        // 比较差异（暂时不比较全部传输），激活回调
+                        int cmp = memcmp(t_read_data, cifx_handle->m_cifx_read_data_buffer[i].data, WHEEL_INPUT_AREA_SIZE);
+                        if(/*cmp != 0 &&*/ cifx_handle->m_read_callback != nullptr && cifx_handle->mp_callback_owner != nullptr){
+                            memcpy(cifx_handle->m_cifx_read_data_buffer[i].data, t_read_data, WHEEL_INPUT_AREA_SIZE);
+                            AREA_DATA_BLOCK t_read_block = cifx_handle->m_cifx_read_data_buffer[i];
+                            cifx_handle->m_read_callback(t_read_block, cifx_handle->mp_callback_owner);
+                        }
+                        cifx_handle->m_read_state.state = SUCCESS;
+                        cifx_handle->m_read_state.update_time = std::chrono::steady_clock::now();
+                        // std::cout << "cifx read success" << std::endl;
+                    }
+                }
+            }
+            // cifx_handle->m_cifx_read_mutex.unlock();
+
+            // 写入数据到PLC
+            // cifx_handle->m_cifx_write_mutex.lock();
+            // std::cout<<"write2 queue size: "<<cifx_handle->m_write_cmd_queue.size()<<std::endl;
+            while (cifx_handle->m_write_cmd_queue.size()!=0)
+            {
+                // std::cout << " 123 " << std::endl;
+                WRITE_CMD_DATA_BLOCK t_write_block = cifx_handle->m_write_cmd_queue.front();
+                int32_t lRet = xChannelIOWrite(cifx_handle->m_cifx_driver_handle,
+                                               t_write_block.area_number, t_write_block.offset, t_write_block.data_length, t_write_block.data, 10);
+                // std::cout << " 1234 " << std::endl;
+                if (lRet != CIFX_NO_ERROR)
+                {
+                    if(cifx_handle->m_write_state.state != CIFX_WRITE_FAILED)
+                        std::cout << "cifx write failed" << std::endl;
+                    cifx_handle->m_write_state.state = CIFX_WRITE_FAILED;
+                    cifx_handle->m_write_state.update_time = std::chrono::steady_clock::now();
+                }
+                else
+                {
+                    cifx_handle->m_write_state.state = SUCCESS;
+                    cifx_handle->m_write_state.update_time = std::chrono::steady_clock::now();
+                    // std::cout << "cifx write success" << std::endl;
+                }
+                cifx_handle->m_write_cmd_queue.pop();
+            }
+            // cifx_handle->m_cifx_write_mutex.unlock();
+        }
+        // 连接状态异常，尝试重连
+        else
+        {
+        }
+        //std::this_thread::yield();
+        usleep(1000*4);
+    }
+    std::cout << "线程退出" << std::endl;
+    cifx_handle->mb_thread_exit = true;
+}
+
+void data_debug(char *data, int length)
+{
+    char buf[255] = {0};
+    sprintf(buf, "current data: ");
+    for (size_t i = 0; i < length; i++)
+    {
+        sprintf(buf + strlen(buf), "\t%u", uint8_t(*(data + i)));
+    }
+    // sprintf(buf + strlen(buf), "\n");
+    std::cout << buf << std::endl;
+}
+
+// int main()
+// {
+//     Cifx_communicator *cc = Cifx_communicator::get_instance();
+//     Error_manager ec = cc->cifx_init();
+//     std::cout << "init state: " << cc->is_initailized() << std::endl;
+
+//     usleep(1000 * 50);
+//     if (ec == SUCCESS)
+//     {
+//         char read_buffer[8] = {0};
+//         char write_buffer[8] = {2};
+//         for (size_t i = 0; i < 1000000; i++)
+//         {
+//             ec = cc->cifx_read(0, 0, 8, read_buffer);
+//             if (ec == SUCCESS)
+//             {
+//                 data_debug(read_buffer, sizeof(read_buffer));
+//             }
+//             else
+//             {
+//                 std::cout << ec.to_string() << std::endl;
+//             }
+
+//             ec = cc->cifx_write(0, 0, 8, read_buffer);
+//             // if (ec == SUCCESS)
+//             // {
+//             //     data_debug(read_buffer, sizeof(read_buffer));
+//             // }
+//             // else
+//             // {
+//             //     std::cout << ec.to_string() << std::endl;
+//             // }
+//             usleep(1000 * 2);
+//         }
+//         usleep(1000*2000);
+//         std::cout << "---------------------end------------------" << std::endl;
+//     }
+//     Cifx_communicator::release_instance();
+// }

src/agv_bundle/src/odom/cifx_communicator.h

@@ -0,0 +1,153 @@
+#ifndef CIFX_COMMUNICATOR_HH
+#define CIFX_COMMUNICATOR_HH
+#include "cifxlinux.h"
+#include "rcX_Public.h"
+#include <unistd.h>
+#include <iostream>
+#include <queue>
+#include <mutex>
+#include <vector>
+#include <thread>
+#include <atomic>
+#include <chrono>
+#include "../error_code/error_code.h"
+namespace agv
+{
+    /*
+    * @Description: cifx RE 50 PNS slave, communicate with PLC through Profinet IO
+    * @Author: yct
+    * @Date: 2020-04-26 13:40:07
+    * @LastEditTime: 2020-04-28 17:41:07
+    * @LastEditors: yct
+    */
+    class Cifx_communicator
+    {
+
+    // 默认cifx设备名称
+    #define CIFX_DEV "cifX0"
+    #define MAX_DATA_SIZE 1024
+    #define WHEEL_INPUT_AREA 0
+    #define WHEEL_INPUT_AREA_SIZE 8
+    #define WHEEL_OUTPUT_AREA 0 
+    #define WHEEL_OUTPUT_AREA_SIZE 8
+
+    public:
+    // cifx 初始化状态，指示已完成几个初始化步骤，用于析构调用
+    enum cifx_state
+    {
+        CIFX_UNINITIALIZED=-1,
+        CIFX_INSTANCE_CREATED,
+        CIFX_DRIVER_INITAILIZED,
+        CIFX_DRIVER_OPENED,
+        CIFX_CHANNEL_OPENED,
+        CIFX_BUS_IS_ON,
+    };
+
+    // 特定区域的数据结构块
+    typedef struct area_data_block
+    {
+    public: 
+        area_data_block(uint32_t area_number){
+            this->area_number = area_number;
+            memset(this->data, 0, MAX_DATA_SIZE);
+        }
+        area_data_block(uint32_t area_number, uint32_t data_length, char *data){
+            this->area_number = area_number;
+            memcpy(this->data, data, std::min((uint32_t)MAX_DATA_SIZE, data_length));
+        }
+        struct area_data_block & operator=(struct area_data_block area_data)
+        {
+            this->area_number = area_data.area_number;
+            memcpy(this->data, area_data.data, (uint32_t)MAX_DATA_SIZE);
+        }
+        uint32_t area_number;
+        char data[MAX_DATA_SIZE];
+    } AREA_DATA_BLOCK;
+
+    // 包含所有待写入指令与信息的数据块
+    typedef struct write_cmd_data_block
+    {
+    public:
+        write_cmd_data_block(uint32_t area_number, uint32_t offset, uint32_t data_length, char *data)
+        {
+            this->area_number = area_number;
+            this->offset = offset;
+            this->data_length = data_length;
+            memcpy(this->data, data, std::min((uint32_t)MAX_DATA_SIZE, data_length));
+        }
+        struct write_cmd_data_block & operator=(struct write_cmd_data_block write_block)
+        {
+            this->area_number = write_block.area_number;
+            this->offset = write_block.offset;
+            this->data_length = write_block.data_length;
+            memcpy(this->data, write_block.data, write_block.data_length);
+            return *this;
+        }
+        uint32_t area_number;
+        uint32_t offset;
+        uint32_t data_length;
+        char data[MAX_DATA_SIZE];
+    } WRITE_CMD_DATA_BLOCK;
+
+    // 读或写异常与时间记录
+    typedef struct state_record
+    {
+    public:
+        state_record()
+        {
+            state = SUCCESS;
+            update_time = std::chrono::steady_clock::now();
+        }
+        Error_manager state;
+        std::chrono::steady_clock::time_point update_time;
+    }STATE_RECORD;
+
+    typedef void (*CIFX_READ_CALLBACK)(AREA_DATA_BLOCK& read_data, void* pOwner);
+
+    public:
+        // 全局获取单例
+        static Cifx_communicator* get_instance();
+        // 全局释放
+        static void release_instance();
+        // 初始化函数
+        Error_manager cifx_init();
+        // 通过cifx将数据从PLC读入
+        Error_manager cifx_read(uint32_t area_number, uint32_t offset, uint32_t data_length, char *data);
+        // 通过cifx将数据写入PLC
+        Error_manager cifx_write(uint32_t area_number, uint32_t offset, uint32_t data_length, char *data);
+        // 获取初始化状态
+        bool is_initialized();
+        // 设置读数据回调，获取到不同数据时激活
+        Error_manager set_read_data_callback(CIFX_READ_CALLBACK callback, void* pOwner);
+
+    private:
+        // 无参构造函数
+        Cifx_communicator();
+        // 析构函数
+        ~Cifx_communicator();
+        // 读写线程函数
+        static void data_exchange(Cifx_communicator* cifx_handle);
+        
+
+    private:
+        static Cifx_communicator* g_cifx_ins; // cifx通信单例句柄
+        static std::mutex g_cifx_access_mutex; // cifx访问互斥锁
+        struct CIFX_LINUX_INIT m_init_params; // 初始化参数
+        CIFXHANDLE m_cifx_driver_handle; // cifx 通道句柄
+        // std::mutex m_cifx_read_mutex; // cifx 读互斥锁
+        // std::mutex m_cifx_write_mutex; // cifx 写互斥锁
+        STATE_RECORD m_read_state; // cifx 读异常标志
+        STATE_RECORD m_write_state; // cifx 写异常标志
+        std::vector<AREA_DATA_BLOCK> m_cifx_read_data_buffer; // cifx 读数据缓存区, 下标分别代表 area_number 与 offset
+        std::queue<WRITE_CMD_DATA_BLOCK> m_write_cmd_queue; // cifx 待写入数据队列
+
+        std::thread* m_data_exchange_thread; // 数据交换线程
+        bool mb_exit; // 系统退出标记
+        bool mb_thread_exit; // 线程退出标志
+        bool mb_initialized; // 系统初始化完成标志
+        int m_cifx_state; // 系统状态，即已初始化完成几个步骤
+        CIFX_READ_CALLBACK m_read_callback; // 读回调成员
+        void * mp_callback_owner; // 回调拥有者
+    };
+}
+#endif //CIFX_COMMUNICATOR_HH

src/agv_bundle/src/odom_node.cpp

@@ -0,0 +1,154 @@
+/*
+ * @Description: 里程计节点，发布odom数据，接收cmd_vel话题下发plc
+ * @Author: yct
+ * @Date: 2020-06-11 17:27:41
+ * @LastEditTime: 2020-06-11 21:42:54
+ * @LastEditors: yct
+ */
+#include <iostream>
+#include <chrono>
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+#include <nav_msgs/Odometry.h>
+#include <Eigen/Core>
+
+#include "odom/Odom.h"
+using namespace agv;
+
+ros::Publisher g_odom_pub;
+// 两主动轮距离
+double g_wheelbase = 0.055;
+
+double deg2rad(double deg)
+{
+  return deg / 180.0 * M_PI;
+}
+
+double rad2deg(double rad)
+{
+  return rad / M_PI * 180.0;
+}
+
+class Odom_test
+{
+
+public:
+  Odom_test()
+  {
+	if(mp_odom == nullptr)
+    		mp_odom = new Odom();
+  }
+  ~Odom_test()
+  {
+    if (mp_odom != nullptr)
+    {
+      delete mp_odom;
+      mp_odom = nullptr;
+    }
+  }
+  bool init()
+  {
+    if (mp_odom != nullptr)
+    {
+      mp_odom->set_driver_data_callback(get_data_callback, this);
+      Error_manager ec = mp_odom->init();
+      if (ec == SUCCESS)
+        return true;
+      std::cout << "odom init: " << ec.to_string() << std::endl;
+    }
+    else
+    {
+      std::cout << "NULL POINTER" << std::endl;
+    }
+    return false;
+  }
+
+  // 待下发cmd_vel消息回调，将其转换为两轮速度下发plc
+  static void CmdVelCallback(const geometry_msgs::Twist::ConstPtr &msg)
+  {
+    if (mp_odom != nullptr)
+    {
+      Eigen::Vector2d speed;
+      Eigen::Vector2d wheel;
+      speed << msg->linear.x, msg->angular.z;
+      speed2wheel(speed, wheel);
+      Driver_data data;
+      data.front_motor_data[0] = wheel[0];
+      data.front_motor_data[1] = wheel[1];
+      Error_manager ec = mp_odom->write_driver_data(data);
+      if (ec != SUCCESS)
+        std::cout << "write error: " << ec.to_string() << std::endl;
+      else{
+		char buf[255];
+	    memset(buf, 0, 255);
+	    sprintf(buf + strlen(buf), "write   \tfront: %2.3f, %2.3f ", data.front_motor_data[0], data.front_motor_data[1]);
+	    // sprintf(buf+strlen(buf), "rear: %2.3f, %2.3f", data.rear_motor_data[0], data.rear_motor_data[1]);
+	    std::cout << buf << std::endl;
+      }
+    }
+    else
+    {
+      std::cout << "NULL POINTER" << std::endl;
+    }
+  }
+
+  // 线角速度转两轮速度
+  static void speed2wheel(Eigen::Vector2d vvth, Eigen::Vector2d &v1v2)
+  {
+    v1v2[0] = vvth[0] - vvth[1] * g_wheelbase / 2.0;
+    v1v2[1] = vvth[0] + vvth[1] * g_wheelbase / 2.0;
+  }
+
+  // 两轮速度转线角速度
+  static void wheel2speed(Eigen::Vector2d v1v2, Eigen::Vector2d &vvth)
+  {
+    vvth[0] = v1v2.sum() / 2.0;
+    vvth[1] = (v1v2[1] - v1v2[0]) / g_wheelbase;
+  }
+
+  static void get_data_callback(Driver_data &data, void *pOwner)
+  {
+    static int read_callback_count = 0;
+    read_callback_count++;
+    char buf[255];
+    memset(buf, 0, 255);
+    sprintf(buf + strlen(buf), "index: %d \t", read_callback_count);
+    sprintf(buf + strlen(buf), "front: %2.3f, %2.3f \t", data.front_motor_data[0], data.front_motor_data[1]);
+    // sprintf(buf+strlen(buf), "rear: %2.3f, %2.3f", data.rear_motor_data[0], data.rear_motor_data[1]);
+    std::cout << buf << std::endl;
+
+    Eigen::Vector2d wheel, speed;
+    wheel << data.front_motor_data[0], data.front_motor_data[1];
+    speed2wheel(wheel, speed);
+
+    nav_msgs::Odometry t_odom;
+    t_odom.child_frame_id = "base_link";
+    t_odom.header.frame_id = "map";
+    t_odom.header.stamp = ros::Time::now();
+    t_odom.header.seq = read_callback_count;
+    t_odom.twist.twist.linear.x = speed[0];
+    t_odom.twist.twist.angular.z = speed[1];
+    g_odom_pub.publish(t_odom);
+  }
+
+  static Odom *mp_odom;
+};
+Odom *Odom_test::mp_odom = nullptr;
+//主函数
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "odom_node");
+
+  ros::NodeHandle gn;
+
+  Odom_test ot;
+  ot.init();
+
+  ros::Subscriber vel_sub_ = gn.subscribe<geometry_msgs::Twist>("cmd_vel", 1, &Odom_test::CmdVelCallback); //move_base_simple
+
+  g_odom_pub = gn.advertise<nav_msgs::Odometry>("odom", 1, true);
+
+  ros::spin();
+
+  return 0;
+}

src/robot_control/CMakeLists.txt

@@ -0,0 +1,54 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(robot_control)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+
+
+catkin_package(
+)
+find_package(catkin REQUIRED COMPONENTS
+        roscpp
+        rospy
+        std_msgs
+        tf
+        nav_msgs
+        )
+
+        
+#aux_source_directory(src/cifx CIFX_SRC)
+#MESSAGE(WARN "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n${CIFX_SRC}")
+
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+  #/usr/local/include/cifx
+  #${CIFX_SRC}
+)
+
+#if(CIFX_HEADER OR CIFX_INC_LIB)
+#    set(LIBCIFX_INCLUDE_DIRS  ${CIFX_HEADER})
+#    set(LIBCIFX_LIBRARIES "-pthread -lrt -lcifx -L${CIFX_INC_LIB}")
+#else(CIFX_HEADER OR CIFX_INC_LIB)
+#    include(FindPkgConfig)
+#    pkg_check_modules(LIBCIFX REQUIRED cifx)
+#endif(CIFX_HEADER OR CIFX_INC_LIB)
+
+# cifx 通信卡
+#add_executable(${PROJECT_NAME}_node src/robot_control_node.cpp src/Agv_card.cpp ${CIFX_SRC})
+#link_directories("/usr/local/lib")
+#target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES}) ${LIBCIFX_LIBRARIES})
+
+# 小乌龟
+# add_executable(${PROJECT_NAME}_node src/robot_control_node.cpp src/turtle_robot.h)
+# link_directories("/usr/local/lib")
+# target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES})
+
+# CAN通信
+add_executable(${PROJECT_NAME}_node src/robot_control_node.cpp src/turtle_robot.h)
+link_directories("/usr/local/lib")
+target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES})

src/robot_control/package.xml

@@ -0,0 +1,69 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>robot_control</name>
+  <version>0.0.0</version>
+  <description>The robot_control package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="zx@todo.todo">zx</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/robot_control</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+
+  <build_export_depend>tf</build_export_depend>
+  <build_export_depend>nav_msgs</build_export_depend>
+
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/robot_control/src/Agv_card.cpp

@@ -0,0 +1,197 @@
+#include "Agv_card.h"
+
+Agv_card::Agv_card()
+{
+
+    //laser_base_link_.setOrigin(tf::Vector3(0.365,0.28,0.0));   //urg 20m
+
+    //雷达相对小车坐标系的 tf变换
+    laser_base_link_.setOrigin(tf::Vector3(-1.65,0,0));     //R2000
+    laser_base_link_.setRotation(tf::Quaternion(0,0,0,1));
+
+
+    tf_sub_ = nh_.subscribe<tf2_msgs::TFMessage>("tf", 1, boost::bind(&Agv_card::location_tf_callback, _1, this));
+
+    Error_manager ec = m_cifx.init();
+    printf(ec.to_string().append("\n").c_str());
+    bConnect_=true;
+
+}
+
+Agv_card::~Agv_card()
+{
+
+}
+
+void Agv_card::location_tf_callback(const tf2_msgs::TFMessage::ConstPtr &msg, Agv_card *par)
+{
+    //获取雷达定位tf
+    
+    for (size_t i = 0; i < msg->transforms.size(); i++)
+    {
+        geometry_msgs::TransformStamped temp = msg->transforms[i];
+        if (temp.header.frame_id == "map" && temp.child_frame_id == "laser")
+        {
+            par->laser_pose_.setRotation(tf::Quaternion(temp.transform.rotation.x, temp.transform.rotation.y, temp.transform.rotation.z, temp.transform.rotation.w));
+            par->laser_pose_.setOrigin(tf::Vector3(temp.transform.translation.x, temp.transform.translation.y, temp.transform.translation.z));
+            
+            break;
+        }
+    }
+}
+
+bool Agv_card::get_odom()
+{
+    if(bConnect_==false) return false;
+    
+    base_link_pose_=laser_pose_*laser_base_link_;
+    if(!readSpeed())
+        return false;
+    if(!wheelsToPoint(current_linear_speed_, current_angular_speed_)) return false;
+    std::cout<<"current v vth: \t\t\t\t "<<current_linear_speed_<<", "<<current_angular_speed_<<std::endl;
+    return true;
+}
+
+bool Agv_card::post_tf()
+{
+    static tf::TransformBroadcaster br;
+    tf::Transform tf;
+    tf.setOrigin(tf::Vector3(0,0,0));
+    tf.setRotation(tf::Quaternion(0,0,0,1));
+    br.sendTransform(tf::StampedTransform(tf, ros::Time::now(), "map", "odom"));
+    br.sendTransform(tf::StampedTransform(base_link_pose_, ros::Time::now(), "odom", "base_link"));
+
+    // tf.setOrigin(tf::Vector3(8.65,1.54,0));
+    // tf.setRotation(tf::Quaternion(0,0,0,1));
+    // br.sendTransform(tf::StampedTransform(tf, ros::Time::now(), "map", "laser"));
+
+    
+}
+
+bool Agv_card::down_speed()
+{
+    writeSpeed(cmd_vel_linear_, cmd_vel_angular_);
+    return true;
+}
+
+bool Agv_card::readSpeed()
+{
+    if(bConnect_==false) return false;
+    if(!m_cifx.is_initialized()) return false;
+    /*
+     * 读取两轮速度值
+     */
+
+    /*ws_load.right_linear_speed=m_card.ReadData(1000);
+    ws_load.left_linear_speed = m_card.ReadData(1001);*/
+    ws_load.right_linear_speed = m_cifx.m_driver_data.front_motor_data[0];
+    ws_load.left_linear_speed = m_cifx.m_driver_data.front_motor_data[1];
+    // printf("read data \t\t\t\t %.3f %.3f\n",ws_load.left_linear_speed, ws_load.right_linear_speed);
+    return true;
+}
+
+bool Agv_card::writeSpeed(const double &linear_speed, const double &angular_speed)
+{
+    if(!point2Wheels(linear_speed, angular_speed)) return false;
+    /*
+     * 下发两轮速度值
+     */
+
+    /*m_card.WriteData(1002,ws_down.left_linear_speed);
+    m_card.WriteData(1003,ws_down.right_linear_speed);*/
+    // static int my_count = 0;
+    if(m_cifx.is_initialized()){
+        Driver_data t_data;
+        t_data.front_motor_data[1] = ws_down.left_linear_speed;
+        t_data.front_motor_data[0] = ws_down.right_linear_speed;
+        // if(my_count++ < 20*3)
+        // {
+        //     t_data.front_motor_data[1] = 0;
+        //     t_data.front_motor_data[0] = 0;
+        // }else if(my_count < 20*6)
+        // {
+        //     t_data.front_motor_data[1] = 0.2;
+        //     t_data.front_motor_data[0] = 0.2;
+        // }
+        // else if(my_count < 20*9)
+        // {
+        //     t_data.front_motor_data[1] = 0.5;
+        //     t_data.front_motor_data[0] = 0.5;
+        // }
+        // else if(my_count < 20*12)
+        // {
+        //     t_data.front_motor_data[1] = 0.1;
+        //     t_data.front_motor_data[0] = 0.1;
+        // }
+        // else
+        // {
+        //     t_data.front_motor_data[1] = 0;
+        //     t_data.front_motor_data[0] = 0;
+        // }
+        Error_manager ec = m_cifx.write_driver_data(t_data);
+        if(ec != SUCCESS)
+            printf("write failed\n");
+        else
+        {
+            printf("write data \t %.3f %.3f\n",linear_speed, angular_speed);
+        }
+    }
+    
+    return true;
+
+}
+
+bool Agv_card::point2Wheels(const double linear_speed, const double angular_speed)
+{
+    //std::cout<<" recv v="<<linear_speed<<", a="<<angular_speed;
+    double v=linear_speed;
+    double w=angular_speed;
+
+    /*if(fabs(w)>0.02&& fabs(v)<1e-4)
+    {
+        w=w>0?0.02:-0.02;
+    }*/
+
+    //两轮间距
+    double width_robot=0.275*2.0;
+    if (fabs(v) < 1e-2)
+    {
+        ws_down.right_linear_speed = w * width_robot / 2.0;
+        ws_down.left_linear_speed = (-1) * ws_down.right_linear_speed;
+    }
+    else if (fabs(w) < 1e-2)
+    {
+        ws_down.left_linear_speed = ws_down.right_linear_speed = v;
+    }
+    else
+    {
+        ws_down.left_linear_speed = v - w * width_robot / 2.0;
+        ws_down.right_linear_speed = v + w * width_robot / 2.0;
+    }
+    
+    return true;
+}
+bool Agv_card::wheelsToPoint(double &linear_speed, double &angular_speed)
+{
+    double v_l=double(ws_load.left_linear_speed);
+    double v_r=double(ws_load.right_linear_speed);
+    //两轮间距
+    double L=0.275*2;
+    double delta_v=v_r-v_l;
+    if(fabs(delta_v)<1e-2)
+    {
+        linear_speed=(v_l+v_r)/2.0;
+        angular_speed=0;
+    }
+    else
+    {
+        double w=delta_v/(L);
+        linear_speed=(v_l+v_r)/2.0;;//(v_l-w*L1)/2.0;
+        angular_speed=w;
+        
+    }
+
+    /* std::cout<<"v_l:"<<v_l<<"\t v_r:"<<v_r<<"\tv:"<<linear_speed
+                        <<"\t \t v_a:"<<angular_speed<<std::endl;*/
+    return true;
+}

src/robot_control/src/Agv_card.h

@@ -0,0 +1,47 @@
+#ifndef CARD_AGV__HH__
+#define CARD_AGV__HH__
+
+#include <ros/ros.h>
+#include "robot_base.h"
+#include "tf2_msgs/TFMessage.h"
+//#include "cp1616/CP16Card.h"
+#include "cifx/Odom.h"
+#define DB_NUMBER   10
+
+using namespace agv;
+
+class Agv_card : public RobotBase
+{
+public:
+    Agv_card();
+    ~Agv_card();
+    virtual bool get_odom();
+    virtual bool post_tf();
+    bool down_speed();
+protected:
+
+    static void location_tf_callback(const tf2_msgs::TFMessage::ConstPtr &msg, Agv_card* agv);
+    bool point2Wheels(const double linear_speed, const double angular_speed);
+    bool wheelsToPoint(double &linear_speed, double& angular_speed);
+    bool readSpeed();
+    bool writeSpeed(const double &linear_speed, const double &angular_speed);
+    struct wheelStatus{
+        double left_linear_speed;
+        double right_linear_speed;
+        // double left_angular_speed;
+        // double right_angular_speed;
+    };
+public:
+    ros::Subscriber tf_sub_;
+    tf::Pose        laser_pose_;
+    tf::Transform   laser_base_link_;
+    wheelStatus ws_load;
+    wheelStatus ws_down;
+
+    //CP16Card            m_card;
+    Odom m_cifx;
+    bool exit_;
+};
+
+
+#endif // !PLC_AGV__HH__

src/robot_control/src/PLCModbus.cpp

@@ -0,0 +1,168 @@
+#include "PLCModbus.h"
+
+namespace plc_modbus
+{
+//********************** constructor **********************
+CPLCModbus::CPLCModbus(void *pOwnerObject)
+{
+    this->ip = 0;
+    this->port = 0;
+}
+
+CPLCModbus::~CPLCModbus()
+{
+}
+
+//********************** init **********************
+bool CPLCModbus::connect(const char *ip, int port)
+{
+    /* TCP */
+    this->ip = ip;
+    this->port = port;
+    this->ctx = modbus_new_tcp(ip, port);
+    modbus_set_debug(this->ctx, false);
+    if (modbus_connect(this->ctx) == -1)
+    {
+        std::cout << "Connection failed: " << modbus_strerror(errno) << std::endl;
+        modbus_free(this->ctx);
+        return false;
+    }
+    else
+    {
+        std::cout << "connected" << std::endl;
+        if(ctx==NULL){
+            std::cout << "connected NULL" << std::endl;
+            return false;
+        }
+        return true;
+    }
+}
+
+void CPLCModbus::disconnect()
+{
+    /* Close the connection */
+    modbus_close(this->ctx);
+    modbus_free(this->ctx);
+}
+
+bool CPLCModbus::reconnect()
+{
+    CPLCModbus::disconnect();
+    return CPLCModbus::connect(this->ip, this->port);
+}
+
+//********************** basic funcs **********************
+uint16_t CPLCModbus::get_current_stage()
+{
+    uint16_t current_stage[] = {1};
+    bool result = this->read_register(CURRENT_STAGE_ADDRESS, current_stage);
+    if(result && current_stage)
+    {
+        return current_stage[0];
+    }else{
+        return (uint16_t)-1;
+    }
+}
+
+uint16_t CPLCModbus::get_finish_status()
+{
+    uint16_t finish_status[] = {1};
+    bool result = this->read_register(FINISH_TRAJECTORY_ADDRESS, finish_status);
+    if (result && finish_status)
+    {
+        return finish_status[0];
+    }
+    else
+    {
+        return (uint16_t)-1;
+    }
+}
+
+bool CPLCModbus::update_trajectory(uint16_t update_flag, uint16_t *dest)
+{
+    bool result = true;
+    uint16_t updt_flag[1] = {update_flag};
+    if (dest)
+    {
+        result &= this->write_registers(POSE_ADDRESS_START, POSE_COUNT * 2, dest);
+        result &= this->write_register(UPDATE_FLAG_ADDRESS, updt_flag);
+        return result;
+    }else{
+        return false;
+    }
+}
+
+bool CPLCModbus::set_slave(int slave)
+{
+    std::lock_guard<std::mutex> lck (mutex_);
+    int rc = modbus_set_slave(this->ctx, slave);
+    if (rc != 1)
+    {
+        std::cout << "set slave failed: " << rc << "," << modbus_strerror(errno) << std::endl;
+        return false;
+    }
+    else
+    {
+        return true;
+    }
+}
+
+bool CPLCModbus::read_registers(int addr, int nb, uint16_t *dest)
+{
+    std::lock_guard<std::mutex> lck (mutex_);
+    int rc = modbus_read_registers(this->ctx, addr, nb, dest);
+    if (rc != nb)
+    {
+        std::cout << "read failed: " << rc << "," << modbus_strerror(errno) << std::endl;
+        return false;
+    }
+    else
+    {
+        return true;
+    }
+}
+
+bool CPLCModbus::write_registers(int addr, int nb, uint16_t *src)
+{
+    std::lock_guard<std::mutex> lck (mutex_);
+    int rc = modbus_write_registers(this->ctx, addr, nb, src);
+    if (rc != nb)
+    {
+        std::cout << "write failed: " << rc << "," << modbus_strerror(errno) << std::endl;
+        return false;
+    }
+    else
+    {
+        return true;
+    }
+}
+
+bool CPLCModbus::read_register(int addr, uint16_t *dest)
+{
+    std::lock_guard<std::mutex> lck (mutex_);
+    int rc = modbus_read_registers(this->ctx, addr, 1, dest);
+    if (rc != 1)
+    {
+        return false;
+    }
+    else
+    {
+        return true;
+    }
+}
+
+bool CPLCModbus::write_register(int addr, uint16_t *src)
+{
+    std::lock_guard<std::mutex> lck (mutex_);
+    int rc = modbus_write_register(this->ctx, addr, src[0]);
+    if (rc != 1)
+    {
+        return false;
+    }
+    else
+    {
+        return true;
+    }
+}
+
+} // namespace plc_modbus

src/robot_control/src/PLCModbus.h

@@ -0,0 +1,62 @@
+
+#ifndef ___PLC_MOTION__HH
+#define ___PLC_MOTION__HH
+#include <vector>
+#include <time.h>
+#include "modbus.h"
+
+#ifndef _MSC_VER
+#include <unistd.h>
+#endif
+
+#include <string.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <iostream>
+#include <mutex>
+
+namespace plc_modbus
+{
+
+#define ADDRESS_LENGTH 100
+#define ADDRESS_START 0
+#define POSE_ADDRESS_START (30 + ADDRESS_START)
+#define POSE_COUNT 10
+#define UPDATE_FLAG_ADDRESS (0 + ADDRESS_START)
+#define CURRENT_STAGE_ADDRESS (1 + ADDRESS_START)
+#define FINISH_TRAJECTORY_ADDRESS (2 + ADDRESS_START)
+
+#define ADDRESS_LEFT_LINEAR_CURRENT 16
+#define ADDRESS_RIGHT_LINEAR_CURRENT 17
+#define ADDRESS_LEFT_LINEAR_TARGET 21
+#define ADDRESS_RIGHT_LINEAR_TARGET 20
+
+class CPLCModbus
+{
+public:
+  CPLCModbus(void *pOwnerObject = NULL);
+  ~CPLCModbus();
+
+  bool connect(const char *ip, int port);
+  void disconnect();
+  bool reconnect();
+  bool set_slave(int slave);
+  bool update_trajectory(uint16_t update_flag, uint16_t *dest);
+  uint16_t get_current_stage();
+  uint16_t get_finish_status();
+  bool read_registers(int addr, int nb, uint16_t *dest);
+  bool write_registers(int addr, int nb, uint16_t *dest);
+
+  bool read_register(int addr, uint16_t *dest);
+  bool write_register(int addr, uint16_t *dest);
+
+  modbus_t *ctx;
+  const char *ip;
+  int port;
+
+  std::mutex    mutex_;
+
+private:
+    };
+};
+#endif

src/robot_control/src/PlcAgvRobot.cpp

@@ -0,0 +1,156 @@
+#include "PlcAgvRobot.h"
+
+PlcAgvRobot::PlcAgvRobot()
+{
+
+    //laser_base_link_.setOrigin(tf::Vector3(0.365,0.28,0.0));   //urg 20m 
+    laser_base_link_.setOrigin(tf::Vector3(0.29,-0.415,0.0));     //R2000
+    laser_base_link_.setRotation(tf::Quaternion(0,0,0,1));
+
+
+    tf_sub_ = nh_.subscribe<tf2_msgs::TFMessage>("tf", 1, boost::bind(&PlcAgvRobot::location_tf_callback, _1, this));
+    //bool result = plc_.connect("192.168.0.30", 502);
+
+    bConnect_=true;
+
+}
+
+PlcAgvRobot::~PlcAgvRobot()
+{
+
+}
+
+void PlcAgvRobot::location_tf_callback(const tf2_msgs::TFMessage::ConstPtr &msg, PlcAgvRobot *par)
+{
+    //获取雷达定位tf
+    
+    for (size_t i = 0; i < msg->transforms.size(); i++)
+    {
+        geometry_msgs::TransformStamped temp = msg->transforms[i];
+        if (temp.header.frame_id == "map" && temp.child_frame_id == "base_link")
+        {
+            par->laser_pose_.setRotation(tf::Quaternion(temp.transform.rotation.x, temp.transform.rotation.y, temp.transform.rotation.z, temp.transform.rotation.w));
+            par->laser_pose_.setOrigin(tf::Vector3(temp.transform.translation.x, temp.transform.translation.y, temp.transform.translation.z));
+            
+            break;
+        }
+    }
+}
+
+bool PlcAgvRobot::get_odom()
+{
+    if(bConnect_==false) return false;
+    
+    base_link_pose_=laser_pose_*laser_base_link_;
+    if(!readSpeed())
+        return false;
+    if(!wheelsToPoint(current_linear_speed_, current_angular_speed_)) return false;
+    //std::cout<<" read   v= "<<current_linear_speed_<<"  , a= "<<current_angular_speed_<<std::endl;
+    return true;
+}
+
+bool PlcAgvRobot::post_tf()
+{
+    static tf::TransformBroadcaster br;
+    tf::Transform tf;
+    tf.setOrigin(tf::Vector3(0,0,0));
+    tf.setRotation(tf::Quaternion(0,0,0,1));
+    br.sendTransform(tf::StampedTransform(tf, ros::Time::now(), "map", "odom"));
+    br.sendTransform(tf::StampedTransform(laser_base_link_, ros::Time::now(), "odom", "base_link"));
+
+    tf.setOrigin(tf::Vector3(-0.62,0.4975,0));
+    tf.setRotation(tf::Quaternion(0,0,0,1));
+    br.sendTransform(tf::StampedTransform(tf, ros::Time::now(), "base_link", "laser"));
+
+    
+}
+
+bool PlcAgvRobot::down_speed()
+{
+    writeSpeed(cmd_vel_linear_, cmd_vel_angular_);
+    return true;
+}
+
+bool PlcAgvRobot::readSpeed()
+{
+    if(bConnect_==false) return false;
+
+    ws_load.right_linear_speed=m_card.ReadData(1000);
+    ws_load.left_linear_speed = m_card.ReadData(1001);
+    
+    return true;
+}
+bool PlcAgvRobot::writeSpeed(const double &linear_speed, const double &angular_speed)
+{
+    if(!point2Wheels(linear_speed, angular_speed)) return false;
+    m_card.WriteData(1002,ws_down.left_linear_speed);
+    m_card.WriteData(1003,ws_down.right_linear_speed);
+    return true;
+
+}
+
+bool PlcAgvRobot::point2Wheels(const double linear_speed, const double angular_speed)
+{
+    //std::cout<<" recv v="<<linear_speed<<", a="<<angular_speed;
+    double v=linear_speed;
+    double w=angular_speed;
+
+    /*if(fabs(w)>0.02&& fabs(v)<1e-4)
+    {
+        w=w>0?0.02:-0.02;
+    }*/
+    double width_robot=0.475*2.0;
+    if (fabs(v) < 1e-2)
+    {
+        ws_down.right_linear_speed = w * width_robot / 2.0;
+        ws_down.left_linear_speed = (-1) * ws_down.right_linear_speed;
+    }
+    else if (fabs(w) < 1e-2)
+    {
+        ws_down.left_linear_speed = ws_down.right_linear_speed = v;
+    }
+    else
+    {
+        ws_down.left_linear_speed = v - w * width_robot / 2.0;
+        ws_down.right_linear_speed = v + w * width_robot / 2.0;
+    }
+
+    /*if(fabs(w)<1e-2)
+    {
+        ws_down.left_linear_speed = v;
+        ws_down.right_linear_speed = v;
+        return true;
+    }
+    double L1=0.475;
+    double L2=0.475;
+    double R = v/w;
+
+    ws_down.left_linear_speed = w*(R+L1);
+    ws_down.right_linear_speed =w*(R-L2);*/
+    //std::cout<<"v:"<<v<<",w:"<<w<<", R:"<<R<<" down v1="<<ws_down.left_linear_speed<<", v2="<<ws_down.right_linear_speed<<std::endl;
+    
+    return true;
+}
+bool PlcAgvRobot::wheelsToPoint(double &linear_speed, double &angular_speed)
+{
+    double v_l=double(ws_load.left_linear_speed)/1000.0;
+    double v_r=double(ws_load.right_linear_speed)/1000.0;
+    double L=0.475*2;
+    double delta_v=v_r-v_l;
+    if(fabs(delta_v)<1e-2)
+    {
+        linear_speed=(v_l+v_r)/2.0;
+        angular_speed=0;
+    }
+    else
+    {
+        double w=delta_v/(L);
+        linear_speed=(v_l+v_r)/2.0;;//(v_l-w*L1)/2.0;
+        angular_speed=w;
+        
+    }
+
+    /* std::cout<<"v_l:"<<v_l<<"\t v_r:"<<v_r<<"\tv:"<<linear_speed
+                        <<"\t \t v_a:"<<angular_speed<<std::endl;*/
+    return true;
+}

src/robot_control/src/PlcAgvRobot.h

@@ -0,0 +1,43 @@
+#ifndef PLC_AGV__HH__
+#define PLC_AGV__HH__
+
+#include <ros/ros.h>
+#include "robot_base.h"
+#include "tf2_msgs/TFMessage.h"
+#include "cp1616/CP16Card.h"
+#define DB_NUMBER   10
+
+class PlcAgvRobot : public RobotBase
+{
+public:
+    PlcAgvRobot();
+    ~PlcAgvRobot();
+    virtual bool get_odom();
+    virtual bool post_tf();
+    bool down_speed();
+protected:
+
+    static void location_tf_callback(const tf2_msgs::TFMessage::ConstPtr &msg, PlcAgvRobot* par);
+    bool point2Wheels(const double linear_speed, const double angular_speed);
+    bool wheelsToPoint(double &linear_speed, double& angular_speed);
+    bool readSpeed();
+    bool writeSpeed(const double &linear_speed, const double &angular_speed);
+    struct wheelStatus{
+        double left_linear_speed;
+        double right_linear_speed;
+        // double left_angular_speed;
+        // double right_angular_speed;
+    };
+public:
+    ros::Subscriber tf_sub_;
+    tf::Pose        laser_pose_;
+    tf::Transform   laser_base_link_;
+    wheelStatus ws_load;
+    wheelStatus ws_down;
+    //plc_modbus::CPLCModbus plc_;
+    CP16Card            m_card;
+    bool exit_;
+};
+
+
+#endif // !PLC_AGV__HH__

src/robot_control/src/Turtle3Robot.h

@@ -0,0 +1,50 @@
+#ifndef __TURTLE3_ROBOT__H_
+#define __TURTLE3_ROBOT__H_
+
+#include "robot_base.h"
+#include "turtlebot_driver/turtlebot3_motor_driver.h"
+
+class Turtle3Robot: public RobotBase
+{
+private:
+    Turtlebot3MotorDriver motor_driver_;
+    bool b_init_;
+public:
+    Turtle3Robot():b_init_(false)
+    {
+       if(!motor_driver_.init())
+       {
+           ROS_INFO(" turtle  driver init failed...\n");
+           exit(0);
+       }
+       b_init_=true;
+    }
+    ~Turtle3Robot()
+    {
+
+    }
+
+protected:
+    virtual bool get_odom()
+    {
+        if(b_init_==false) return false;
+        int32_t left=0,right=0;
+        bool succ = motor_driver_.readEncoder(left,right);
+
+        if(succ==false) return false;
+
+        ROS_INFO("left:%d, right:%d\n");
+
+        return true;
+    }    
+    virtual bool post_tf(){return false;}
+    virtual bool down_speed()
+    {
+        if(b_init_==false) return false;
+        motor_driver_.speedControl(100, 100);
+    }
+};
+
+
+
+#endif

src/robot_control/src/cifx/Odom.cpp

@@ -0,0 +1,126 @@
+//
+// Created by zx on 2020/4/28.
+//
+
+#include "Odom.h"
+using namespace agv;
+
+// 无参构造
+Odom::Odom()
+{
+    std::cout<<"odom 构造"<<std::endl;
+    mp_cifx_handle = Cifx_communicator::get_instance();
+    mb_initialized = false;
+    m_odom_callback = nullptr;
+    mp_odom_callback_owner = nullptr;
+}
+
+// 析构
+Odom::~Odom()
+{
+    std::cout<<"odom 析构"<<std::endl;
+	Driver_data data;
+    data.front_motor_data[0] = 0;
+    data.front_motor_data[1] = 0;
+    Error_manager ec = write_driver_data(data);
+    if (ec != SUCCESS)
+        std::cout << "write zero speed before close failed: " << ec.to_string() << std::endl;
+    else
+    {
+        std::cout << "successfully write zero speed before close" << std::endl;
+    }
+    
+    if(mp_cifx_handle != nullptr){
+        Cifx_communicator::release_instance();
+    }
+    if(m_odom_callback!=nullptr)
+    {
+        m_odom_callback = nullptr;
+    }
+    if(mp_odom_callback_owner != nullptr)
+    {
+        mp_odom_callback_owner = nullptr;
+    }
+}
+
+// 初始化函数
+Error_manager Odom::init()
+{
+    if (mp_cifx_handle == nullptr)
+        return POINTER_IS_NULL;
+    // 设置cifx读消息回调
+    Error_manager ec = mp_cifx_handle->set_read_data_callback(Odom::get_cifx_data_callback, this);
+    if(ec != SUCCESS)
+        return ec;
+    // cifx通信卡初始化
+    ec = mp_cifx_handle->cifx_init();
+    if(ec != SUCCESS)
+        return ec;
+    mb_initialized = true;
+    return SUCCESS;
+}
+
+// 获取初始化状态
+bool Odom::is_initialized()
+{
+    return mb_initialized;
+}
+
+// 设置通信卡读数据回调
+Error_manager Odom::set_driver_data_callback(ODOM_CALLBACK func, void *pOwner)
+{
+    m_odom_callback = func;
+    mp_odom_callback_owner = pOwner;
+    return SUCCESS;
+}
+
+// 向通信卡写入电机驱动器数据
+Error_manager Odom::write_driver_data(Driver_data &data)
+{
+    if(mp_cifx_handle == nullptr)
+        return POINTER_IS_NULL;
+    char t_write_data[WHEEL_OUTPUT_AREA_SIZE];
+    memset(t_write_data, 0, WHEEL_OUTPUT_AREA_SIZE);
+    for (size_t i = 0; i < WHEEL_OUTPUT_AREA_SIZE; i+=2)
+    {
+        int16_t t_wheel_data = 0;
+        if((i / 2) < MOTOR_NUM){
+            t_wheel_data = static_cast<int16_t>(data.front_motor_data[i/2] * 1000.0);
+        }else{
+            t_wheel_data = static_cast<int16_t>(data.rear_motor_data[i/2-2] * 1000.0);
+        }
+        // 字节序倒序
+        // t_wheel_data = (t_wheel_data << 8 || t_wheel_data >> 8);
+        memcpy(t_write_data+i, &t_wheel_data, sizeof(int16_t));
+    }
+    return mp_cifx_handle->cifx_write(WHEEL_OUTPUT_AREA, 0, WHEEL_OUTPUT_AREA_SIZE, t_write_data);
+}
+
+
+
+// 向通信卡读数据回调
+void Odom::get_cifx_data_callback(Cifx_communicator::AREA_DATA_BLOCK &read_data, void *pOwner)
+{
+    // std::cout<<"odom data callback"<<std::endl;
+    if (pOwner == nullptr)
+        return;
+    Odom *odom = reinterpret_cast<Odom *>(pOwner);
+    Driver_data t_driver_data;
+    for (size_t i = 0; i < WHEEL_OUTPUT_AREA_SIZE; i += 2)
+    {
+        int16_t t_wheel_data = *(reinterpret_cast<uint16_t *>(read_data.data + i));
+        if ((i / 2) < MOTOR_NUM)
+        {
+            t_driver_data.front_motor_data[i / 2] = t_wheel_data / 1000.0;
+        }
+        else
+        {
+            t_driver_data.rear_motor_data[i / 2 - 2] = t_wheel_data / 1000.0;
+        }
+        memcpy(&odom->m_driver_data, &t_driver_data, sizeof(Driver_data));
+    }
+    if (odom->m_odom_callback != nullptr && odom->mp_odom_callback_owner != nullptr)
+    {
+        odom->m_odom_callback(t_driver_data, odom->mp_odom_callback_owner);
+    }
+}

src/robot_control/src/cifx/Odom.h

@@ -0,0 +1,44 @@
+//
+// Created by zx on 2020/4/28.
+//
+
+#ifndef AGV_ODOM_H
+#define AGV_ODOM_H
+#include <iostream>
+#include "dtype.h"
+#include "error_code.h"
+#include "cifx_communicator.h"
+
+namespace agv
+{
+    typedef void (*ODOM_CALLBACK)(Driver_data& data,void* pOwner);
+    
+    class Odom {
+    public:
+        // 无参构造
+        Odom();
+        // 析构
+        ~Odom();
+        // 初始化函数
+        Error_manager init();
+        // 设置通信卡读数据回调
+        Error_manager set_driver_data_callback(ODOM_CALLBACK func,void* pOwner);
+        // 向通信卡写入电机驱动器数据
+        Error_manager write_driver_data(Driver_data& data);
+        // 获取初始化状态
+        bool is_initialized();
+    private:
+        // 向通信卡读数据回调
+        static void get_cifx_data_callback(Cifx_communicator::AREA_DATA_BLOCK& read_data, void* pOwner);
+
+    private:
+        Cifx_communicator* mp_cifx_handle; // cifx 通信卡句柄
+        ODOM_CALLBACK m_odom_callback; // odom 获取新消息回调
+        void* mp_odom_callback_owner; // odom 回调拥有者
+        bool mb_initialized; // 初始化状态
+    public:
+        Driver_data m_driver_data; // 保存读取的两轮速度
+    };
+
+}
+#endif //AGV_ODOM_H

src/robot_control/src/cifx/cifx_communicator.cpp

@@ -0,0 +1,366 @@
+/*
+ * @Description: cifx RE 50 PNS slave, communicate with PLC through Profinet IO
+ * @Author: yct
+ * @Date: 2020-04-26 13:39:47
+ * @LastEditTime: 2020-04-28 18:01:51
+ * @LastEditors: yct
+ */
+#include "cifx_communicator.h"
+using namespace agv;
+
+Cifx_communicator *Cifx_communicator::g_cifx_ins = nullptr;
+std::mutex Cifx_communicator::g_cifx_access_mutex;
+
+/*****************************************************************************/
+/*! Displays cifX error
+*   \param lError     Error code                                             */
+/*****************************************************************************/
+std::string show_error(int32_t lError)
+{
+    if (lError != CIFX_NO_ERROR)
+    {
+        char szError[1024] = {0};
+        char buf[1152] = {0};
+        xDriverGetErrorDescription(lError, szError, sizeof(szError));
+        sprintf(buf, "Error: 0x%X, <%s>\n", (unsigned int)lError, szError);
+        return std::string(buf);
+    }
+    return std::string("No Error\n");
+}
+
+// 获取通信卡实例
+Cifx_communicator *Cifx_communicator::get_instance()
+{
+    if (g_cifx_ins == nullptr)
+    {
+        std::lock_guard<std::mutex> lck(g_cifx_access_mutex);
+        if (g_cifx_ins == nullptr)
+        {
+            g_cifx_ins = new Cifx_communicator();
+        }
+    }
+    return g_cifx_ins;
+}
+
+// 释放单例
+void Cifx_communicator::release_instance()
+{
+    std::lock_guard<std::mutex> lck(g_cifx_access_mutex);
+    if(g_cifx_ins != nullptr){
+        std::cout<<"cifx 释放"<<std::endl;
+        delete g_cifx_ins;
+        g_cifx_ins = nullptr;
+    }
+}
+
+// 初始化通信卡
+Error_manager Cifx_communicator::cifx_init()
+{
+    m_cifx_state = CIFX_INSTANCE_CREATED;
+    // 清除数据
+    while (m_write_cmd_queue.size() != 0)
+        m_write_cmd_queue.pop();
+    m_cifx_read_data_buffer.clear();
+    // 初始化读写数据块
+    m_cifx_read_data_buffer.push_back(AREA_DATA_BLOCK(WHEEL_INPUT_AREA));
+
+    // 初始化驱动，linux下特有
+    int32_t lRet = cifXDriverInit(&m_init_params);
+    std::string error_str = show_error(lRet);
+    if (lRet != CIFX_NO_ERROR)
+    {
+        return Error_manager(CIFX_DRIVER_INIT_ERROR, Error_level::MAJOR_ERROR, error_str.c_str(), error_str.length());
+    }
+    m_cifx_state = CIFX_DRIVER_INITAILIZED;
+
+    // 开启驱动
+    lRet = xDriverOpen(&m_cifx_driver_handle);
+    error_str = show_error(lRet);
+    if (lRet != CIFX_NO_ERROR)
+    {
+        return Error_manager(CIFX_DRIVER_OPEN_ERROR, Error_level::MAJOR_ERROR, error_str.c_str(), error_str.length());
+    }
+    m_cifx_state = CIFX_DRIVER_OPENED;
+    usleep(1000 * 40); // 驱动开启后需要约40ms的等待时间
+
+    // 开启通道
+    lRet = xChannelOpen(NULL, CIFX_DEV, 0, &m_cifx_driver_handle);
+    error_str = show_error(lRet);
+    if (lRet != CIFX_NO_ERROR)
+    {
+        return Error_manager(CIFX_CHANNEL_OPEN_ERROR, Error_level::MAJOR_ERROR, error_str.c_str(), error_str.length());
+    }
+    m_cifx_state = CIFX_CHANNEL_OPENED;
+
+    // 开启通信总线
+    unsigned long ulState;
+    lRet = xChannelBusState(m_cifx_driver_handle, CIFX_BUS_STATE_ON, (TLR_UINT32 *)&ulState, 10000);
+    error_str = show_error(lRet);
+    if (lRet != CIFX_NO_ERROR)
+    {
+        return Error_manager(CIFX_BUS_OPEN_ERROR, Error_level::MAJOR_ERROR, error_str.c_str(), error_str.length());
+    }
+    m_cifx_state = CIFX_BUS_IS_ON;
+
+    m_data_exchange_thread = new std::thread(&Cifx_communicator::data_exchange, g_cifx_ins);
+    m_data_exchange_thread->detach();
+    mb_initialized = true;
+    return SUCCESS;
+}
+
+// 通过cifx将数据从PLC读入
+Error_manager Cifx_communicator::cifx_read(uint32_t area_number, uint32_t offset, uint32_t data_length, char *data)
+{
+    if (data_length + offset > MAX_DATA_SIZE)
+        return PARAMETER_ERROR;
+    // std::lock_guard<std::mutex> lck(m_cifx_read_mutex);
+    if (m_read_state.state != SUCCESS)
+        return CIFX_READ_FAILED;
+    if (std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - m_read_state.update_time).count() > 20000)
+        return CIFX_READ_TIMEOUT;
+    bool read_success = false;
+    for (size_t i = 0; i < m_cifx_read_data_buffer.size(); i++)
+    {
+        if (m_cifx_read_data_buffer[i].area_number == area_number)
+        {
+            memcpy(data, m_cifx_read_data_buffer[i].data + offset, data_length);
+            read_success = true;
+            break;
+        }
+    }
+    if (!read_success)
+    {
+        return PARAMETER_ERROR;
+    }
+    else
+    {
+        return SUCCESS;
+    }
+}
+
+// 通过cifx将数据写入PLC
+Error_manager Cifx_communicator::cifx_write(uint32_t area_number, uint32_t offset, uint32_t data_length, char *data)
+{
+    // std::lock_guard<std::mutex> lck(m_cifx_write_mutex);
+    if (m_write_state.state != SUCCESS)
+        return CIFX_WRITE_FAILED;
+    // if (std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - m_write_state.update_time).count() > 20000)
+    //     return CIFX_WRITE_TIMEOUT;
+    m_write_cmd_queue.push(WRITE_CMD_DATA_BLOCK(area_number, offset, data_length, data));
+    // std::cout<<"write queue size: "<<m_write_cmd_queue.size()<<std::endl;
+    return SUCCESS;
+}
+
+// 获取初始化状态
+bool Cifx_communicator::is_initialized()
+{
+    return mb_initialized;
+}
+
+// 无参构造函数
+Cifx_communicator::Cifx_communicator()
+{
+    m_read_callback = nullptr;
+    mp_callback_owner = nullptr;
+    m_cifx_state = CIFX_UNINITIALIZED;
+    mb_exit = false;
+    mb_thread_exit = false;
+    mb_initialized = false;
+    m_data_exchange_thread = nullptr;
+    m_cifx_driver_handle = nullptr;
+    m_init_params.init_options = CIFX_DRIVER_INIT_AUTOSCAN;
+    m_init_params.iCardNumber = 0;
+    m_init_params.fEnableCardLocking = 0;
+    m_init_params.base_dir = NULL;
+    m_init_params.poll_interval = 0;
+    m_init_params.poll_StackSize = 0; /* set to 0 to use default */
+    m_init_params.trace_level = 255;
+    m_init_params.user_card_cnt = 0;
+    m_init_params.user_cards = NULL;
+    m_init_params.poll_priority = 0;
+}
+
+// 析构函数
+Cifx_communicator::~Cifx_communicator()
+{
+    mb_exit = true;
+    unsigned long ulState;
+    while(m_data_exchange_thread != nullptr && !mb_thread_exit){
+        // std::cout << "等待线程退出" << std::endl;
+        usleep(1);
+    }
+    std::cout << "析构线程" << std::endl;
+    if (m_data_exchange_thread != nullptr)
+    {
+        if (m_data_exchange_thread->joinable())
+        {
+            m_data_exchange_thread->join();
+        }
+        delete m_data_exchange_thread;
+        m_data_exchange_thread = nullptr;
+    }
+    std::cout << "释放SDK" << std::endl;
+    // 顺序释放cifx通信卡SDK
+    if (m_cifx_state == CIFX_BUS_IS_ON)
+        xChannelBusState(m_cifx_driver_handle, CIFX_BUS_STATE_OFF, (TLR_UINT32 *)&ulState, 10000);
+    if (m_cifx_state == CIFX_CHANNEL_OPENED)
+        xChannelClose(m_cifx_driver_handle);
+    if (m_cifx_state == CIFX_DRIVER_OPENED)
+        xDriverClose(m_cifx_driver_handle);
+    if (m_cifx_state == CIFX_DRIVER_INITAILIZED)
+        cifXDriverDeinit();
+}
+
+// 设置读数据回调，获取到不同数据时激活
+Error_manager Cifx_communicator::set_read_data_callback(CIFX_READ_CALLBACK callback, void* pOwner)
+{
+    m_read_callback = callback;
+    mp_callback_owner = pOwner;
+    return SUCCESS;
+}
+
+// 读写线程函数
+void Cifx_communicator::data_exchange(Cifx_communicator *cifx_handle)
+{
+    std::cout << "线程启动" << std::endl;
+    if (cifx_handle == nullptr)
+    {
+        std::cout << "空指针" << std::endl;
+        return;
+    }
+    if (!cifx_handle->is_initialized())
+    {
+        std::cout << "未初始化" << std::endl;
+        return;
+    }
+    while (!cifx_handle->mb_exit)
+    {
+        // 连接状态正常
+        if (cifx_handle->m_cifx_state == CIFX_BUS_IS_ON)
+        {
+            // std::cout << " 000000 " << std::endl;
+            // 读取PLC数据
+            // cifx_handle->m_cifx_read_mutex.lock();
+            for (size_t i = 0; i < cifx_handle->m_cifx_read_data_buffer.size(); i++)
+            {
+                if (cifx_handle->m_cifx_read_data_buffer[i].area_number == WHEEL_INPUT_AREA)
+                {
+                    // std::cout << " 000111 " << std::endl;
+                    char t_read_data[WHEEL_INPUT_AREA_SIZE]={0};
+                    int32_t lRet = xChannelIORead(cifx_handle->m_cifx_driver_handle,
+                                                  WHEEL_INPUT_AREA, 0, WHEEL_INPUT_AREA_SIZE, t_read_data, 10);
+                    // std::cout << " 000222 " << std::endl;
+                    if (lRet != CIFX_NO_ERROR)
+                    {
+                        if(cifx_handle->m_read_state.state != CIFX_READ_FAILED){
+                            std::cout << "cifx read failed: \n" << lRet << std::endl;
+                        }
+                        cifx_handle->m_read_state.state = CIFX_READ_FAILED;
+                        cifx_handle->m_read_state.update_time = std::chrono::steady_clock::now();
+                    }
+                    else
+                    {
+                        // 比较差异（暂时不比较全部传输），激活回调
+                        int cmp = memcmp(t_read_data, cifx_handle->m_cifx_read_data_buffer[i].data, WHEEL_INPUT_AREA_SIZE);
+                        if(/*cmp != 0 &&*/ cifx_handle->m_read_callback != nullptr && cifx_handle->mp_callback_owner != nullptr){
+                            memcpy(cifx_handle->m_cifx_read_data_buffer[i].data, t_read_data, WHEEL_INPUT_AREA_SIZE);
+                            AREA_DATA_BLOCK t_read_block = cifx_handle->m_cifx_read_data_buffer[i];
+                            cifx_handle->m_read_callback(t_read_block, cifx_handle->mp_callback_owner);
+                        }
+                        cifx_handle->m_read_state.state = SUCCESS;
+                        cifx_handle->m_read_state.update_time = std::chrono::steady_clock::now();
+                        // std::cout << "cifx read success" << std::endl;
+                    }
+                }
+            }
+            // cifx_handle->m_cifx_read_mutex.unlock();
+
+            // 写入数据到PLC
+            // cifx_handle->m_cifx_write_mutex.lock();
+            // std::cout<<"write2 queue size: "<<cifx_handle->m_write_cmd_queue.size()<<std::endl;
+            while (cifx_handle->m_write_cmd_queue.size()!=0)
+            {
+                // std::cout << " 123 " << std::endl;
+                WRITE_CMD_DATA_BLOCK t_write_block = cifx_handle->m_write_cmd_queue.front();
+                int32_t lRet = xChannelIOWrite(cifx_handle->m_cifx_driver_handle,
+                                               t_write_block.area_number, t_write_block.offset, t_write_block.data_length, t_write_block.data, 10);
+                // std::cout << " 1234 " << std::endl;
+                if (lRet != CIFX_NO_ERROR)
+                {
+                    if(cifx_handle->m_write_state.state != CIFX_WRITE_FAILED)
+                        std::cout << "cifx write failed: \n" << lRet << std::endl;
+                    cifx_handle->m_write_state.state = CIFX_WRITE_FAILED;
+                    cifx_handle->m_write_state.update_time = std::chrono::steady_clock::now();
+                }
+                else
+                {
+                    cifx_handle->m_write_state.state = SUCCESS;
+                    cifx_handle->m_write_state.update_time = std::chrono::steady_clock::now();
+                    // std::cout << "cifx write success" << std::endl;
+                }
+                cifx_handle->m_write_cmd_queue.pop();
+            }
+            // cifx_handle->m_cifx_write_mutex.unlock();
+        }
+        // 连接状态异常，尝试重连
+        else
+        {
+        }
+        //std::this_thread::yield();
+        usleep(1000*8);
+    }
+    std::cout << "线程退出" << std::endl;
+    cifx_handle->mb_thread_exit = true;
+}
+
+void data_debug(char *data, int length)
+{
+    char buf[255] = {0};
+    sprintf(buf, "current data: ");
+    for (size_t i = 0; i < length; i++)
+    {
+        sprintf(buf + strlen(buf), "\t%u", uint8_t(*(data + i)));
+    }
+    // sprintf(buf + strlen(buf), "\n");
+    std::cout << buf << std::endl;
+}
+
+// int main()
+// {
+//     Cifx_communicator *cc = Cifx_communicator::get_instance();
+//     Error_manager ec = cc->cifx_init();
+//     std::cout << "init state: " << cc->is_initailized() << std::endl;
+
+//     usleep(1000 * 50);
+//     if (ec == SUCCESS)
+//     {
+//         char read_buffer[8] = {0};
+//         char write_buffer[8] = {2};
+//         for (size_t i = 0; i < 1000000; i++)
+//         {
+//             ec = cc->cifx_read(0, 0, 8, read_buffer);
+//             if (ec == SUCCESS)
+//             {
+//                 data_debug(read_buffer, sizeof(read_buffer));
+//             }
+//             else
+//             {
+//                 std::cout << ec.to_string() << std::endl;
+//             }
+
+//             ec = cc->cifx_write(0, 0, 8, read_buffer);
+//             // if (ec == SUCCESS)
+//             // {
+//             //     data_debug(read_buffer, sizeof(read_buffer));
+//             // }
+//             // else
+//             // {
+//             //     std::cout << ec.to_string() << std::endl;
+//             // }
+//             usleep(1000 * 2);
+//         }
+//         usleep(1000*2000);
+//         std::cout << "---------------------end------------------" << std::endl;
+//     }
+//     Cifx_communicator::release_instance();
+// }

src/robot_control/src/cifx/cifx_communicator.h

@@ -0,0 +1,153 @@
+#ifndef CIFX_COMMUNICATOR_HH
+#define CIFX_COMMUNICATOR_HH
+#include "cifxlinux.h"
+#include "rcX_Public.h"
+#include <unistd.h>
+#include <iostream>
+#include <queue>
+#include <mutex>
+#include <vector>
+#include <thread>
+#include <atomic>
+#include <chrono>
+#include "error_code.h"
+namespace agv
+{
+    /*
+    * @Description: cifx RE 50 PNS slave, communicate with PLC through Profinet IO
+    * @Author: yct
+    * @Date: 2020-04-26 13:40:07
+    * @LastEditTime: 2020-04-28 17:41:07
+    * @LastEditors: yct
+    */
+    class Cifx_communicator
+    {
+
+    // 默认cifx设备名称
+    #define CIFX_DEV "cifX0"
+    #define MAX_DATA_SIZE 1024
+    #define WHEEL_INPUT_AREA 0
+    #define WHEEL_INPUT_AREA_SIZE 8
+    #define WHEEL_OUTPUT_AREA 0 
+    #define WHEEL_OUTPUT_AREA_SIZE 8
+
+    public:
+    // cifx 初始化状态，指示已完成几个初始化步骤，用于析构调用
+    enum cifx_state
+    {
+        CIFX_UNINITIALIZED=-1,
+        CIFX_INSTANCE_CREATED,
+        CIFX_DRIVER_INITAILIZED,
+        CIFX_DRIVER_OPENED,
+        CIFX_CHANNEL_OPENED,
+        CIFX_BUS_IS_ON,
+    };
+
+    // 特定区域的数据结构块
+    typedef struct area_data_block
+    {
+    public: 
+        area_data_block(uint32_t area_number){
+            this->area_number = area_number;
+            memset(this->data, 0, MAX_DATA_SIZE);
+        }
+        area_data_block(uint32_t area_number, uint32_t data_length, char *data){
+            this->area_number = area_number;
+            memcpy(this->data, data, std::min((uint32_t)MAX_DATA_SIZE, data_length));
+        }
+        struct area_data_block & operator=(struct area_data_block area_data)
+        {
+            this->area_number = area_data.area_number;
+            memcpy(this->data, area_data.data, (uint32_t)MAX_DATA_SIZE);
+        }
+        uint32_t area_number;
+        char data[MAX_DATA_SIZE];
+    } AREA_DATA_BLOCK;
+
+    // 包含所有待写入指令与信息的数据块
+    typedef struct write_cmd_data_block
+    {
+    public:
+        write_cmd_data_block(uint32_t area_number, uint32_t offset, uint32_t data_length, char *data)
+        {
+            this->area_number = area_number;
+            this->offset = offset;
+            this->data_length = data_length;
+            memcpy(this->data, data, std::min((uint32_t)MAX_DATA_SIZE, data_length));
+        }
+        struct write_cmd_data_block & operator=(struct write_cmd_data_block write_block)
+        {
+            this->area_number = write_block.area_number;
+            this->offset = write_block.offset;
+            this->data_length = write_block.data_length;
+            memcpy(this->data, write_block.data, write_block.data_length);
+            return *this;
+        }
+        uint32_t area_number;
+        uint32_t offset;
+        uint32_t data_length;
+        char data[MAX_DATA_SIZE];
+    } WRITE_CMD_DATA_BLOCK;
+
+    // 读或写异常与时间记录
+    typedef struct state_record
+    {
+    public:
+        state_record()
+        {
+            state = SUCCESS;
+            update_time = std::chrono::steady_clock::now();
+        }
+        Error_manager state;
+        std::chrono::steady_clock::time_point update_time;
+    }STATE_RECORD;
+
+    typedef void (*CIFX_READ_CALLBACK)(AREA_DATA_BLOCK& read_data, void* pOwner);
+
+    public:
+        // 全局获取单例
+        static Cifx_communicator* get_instance();
+        // 全局释放
+        static void release_instance();
+        // 初始化函数
+        Error_manager cifx_init();
+        // 通过cifx将数据从PLC读入
+        Error_manager cifx_read(uint32_t area_number, uint32_t offset, uint32_t data_length, char *data);
+        // 通过cifx将数据写入PLC
+        Error_manager cifx_write(uint32_t area_number, uint32_t offset, uint32_t data_length, char *data);
+        // 获取初始化状态
+        bool is_initialized();
+        // 设置读数据回调，获取到不同数据时激活
+        Error_manager set_read_data_callback(CIFX_READ_CALLBACK callback, void* pOwner);
+
+    private:
+        // 无参构造函数
+        Cifx_communicator();
+        // 析构函数
+        ~Cifx_communicator();
+        // 读写线程函数
+        static void data_exchange(Cifx_communicator* cifx_handle);
+        
+
+    private:
+        static Cifx_communicator* g_cifx_ins; // cifx通信单例句柄
+        static std::mutex g_cifx_access_mutex; // cifx访问互斥锁
+        struct CIFX_LINUX_INIT m_init_params; // 初始化参数
+        CIFXHANDLE m_cifx_driver_handle; // cifx 通道句柄
+        // std::mutex m_cifx_read_mutex; // cifx 读互斥锁
+        // std::mutex m_cifx_write_mutex; // cifx 写互斥锁
+        STATE_RECORD m_read_state; // cifx 读异常标志
+        STATE_RECORD m_write_state; // cifx 写异常标志
+        std::vector<AREA_DATA_BLOCK> m_cifx_read_data_buffer; // cifx 读数据缓存区, 下标分别代表 area_number 与 offset
+        std::queue<WRITE_CMD_DATA_BLOCK> m_write_cmd_queue; // cifx 待写入数据队列
+
+        std::thread* m_data_exchange_thread; // 数据交换线程
+        bool mb_exit; // 系统退出标记
+        bool mb_thread_exit; // 线程退出标志
+        bool mb_initialized; // 系统初始化完成标志
+        int m_cifx_state; // 系统状态，即已初始化完成几个步骤
+        CIFX_READ_CALLBACK m_read_callback; // 读回调成员
+        void * mp_callback_owner; // 回调拥有者
+    };
+}
+#endif //CIFX_COMMUNICATOR_HH

src/robot_control/src/cifx/dtype.h

@@ -0,0 +1,40 @@
+//
+// Created by zx on 2020/4/28.
+//
+
+#ifndef AGV_DTYPE_H
+#define AGV_DTYPE_H
+namespace agv
+{
+/**
+ * 位姿数据类型
+ */
+    class Pose
+    {
+    public:
+        Pose();
+        Pose(double x,double y,double theta);
+        double x();
+        double y();
+        double theta();
+
+    protected:
+        double m_x;
+        double m_y;
+        double m_theta;
+    };
+
+
+    /*
+     * agv各项驱动数据
+     */
+#define MOTOR_NUM   2
+    typedef struct AGV_DRIVER_DATE
+    {
+        double front_motor_data[MOTOR_NUM]; //前车电机数据
+        double rear_motor_data[MOTOR_NUM];  //后车电机数据
+    }Driver_data;
+
+}
+
+#endif //AGV_DTYPE_H

src/robot_control/src/cifx/error_code.cpp

@@ -0,0 +1,526 @@
+
+//Error_code是错误码的底层通用模块，
+//功能：用作故障分析和处理。
+
+//用法：所有的功能接口函数return错误管理类，
+//然后上层判断分析错误码，并进行故障处理。
+
+
+
+#include "error_code.h"
+
+/////////////////////////////////////////////
+//构造函数
+Error_manager::Error_manager()
+{
+    m_error_code = SUCCESS;
+    m_error_level = NORMAL;
+    pm_error_description = 0;
+    m_description_length = 0;
+    return ;
+}
+//拷贝构造
+Error_manager::Error_manager(const Error_manager & error_manager)
+{
+    this->m_error_code = error_manager.m_error_code;
+    this->m_error_level = error_manager.m_error_level;
+    pm_error_description = NULL;
+    m_description_length = 0;
+    reallocate_memory_and_copy_string(error_manager.pm_error_description, error_manager.m_description_length);
+    return ;
+}
+//赋值构造
+Error_manager::Error_manager(Error_code error_code, Error_level error_level,
+    const char* p_error_description, int description_length)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    pm_error_description = NULL;
+    m_description_length = 0;
+    reallocate_memory_and_copy_string(p_error_description, description_length);
+    return ;
+}
+//赋值构造
+Error_manager::Error_manager(Error_code error_code, Error_level error_level , std::string & error_aggregate_string)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    pm_error_description = NULL;
+    m_description_length = 0;
+    reallocate_memory_and_copy_string(error_aggregate_string);
+    return ;
+}
+//析构函数
+Error_manager::~Error_manager()
+{
+    free_description();
+}
+
+//初始化
+void Error_manager::error_manager_init()
+{
+    error_manager_clear_all();
+    return;
+}
+//初始化
+void Error_manager::error_manager_init(Error_code error_code, Error_level error_level, const char* p_error_description, int description_length)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    reallocate_memory_and_copy_string(p_error_description, description_length);
+    return ;
+}
+//初始化
+void Error_manager::error_manager_init(Error_code error_code, Error_level error_level , std::string & error_aggregate_string)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    reallocate_memory_and_copy_string(error_aggregate_string);
+    return ;
+}
+//重置
+void Error_manager::error_manager_reset(Error_code error_code, Error_level error_level, const char* p_error_description, int description_length)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    reallocate_memory_and_copy_string(p_error_description, description_length);
+    return ;
+}
+//重置
+void Error_manager::error_manager_reset(Error_code error_code, Error_level error_level , std::string & error_aggregate_string)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    reallocate_memory_and_copy_string(error_aggregate_string);
+    return ;
+}
+//重置
+void Error_manager::error_manager_reset(const Error_manager & error_manager)
+{
+    this->m_error_code = error_manager.m_error_code;
+    this->m_error_level = error_manager.m_error_level;
+    reallocate_memory_and_copy_string(error_manager.pm_error_description, error_manager.m_description_length);
+    return ;
+}
+//清除所有内容
+void Error_manager::error_manager_clear_all()
+{
+    m_error_code = SUCCESS;
+    m_error_level = NORMAL;
+    free_description();
+}
+
+//重载=
+Error_manager& Error_manager::operator=(const Error_manager & error_manager)
+{
+    error_manager_reset(error_manager);
+}
+//重载=，支持Error_manager和Error_code的直接转化，会清空错误等级和描述
+Error_manager& Error_manager::operator=(Error_code error_code)
+{
+    error_manager_clear_all();
+    set_error_code(error_code);
+}
+//重载==
+bool Error_manager::operator==(const Error_manager & error_manager)
+{
+    is_equal_error_manager(error_manager);
+}
+//重载==，支持Error_manager和Error_code的直接比较
+bool Error_manager::operator==(Error_code error_code)
+{
+    if(m_error_code == error_code)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+//重载!=
+bool Error_manager::operator!=(const Error_manager & error_manager)
+{
+    ! is_equal_error_manager(error_manager);
+}
+//重载!=，支持Error_manager和Error_code的直接比较
+bool Error_manager::operator!=(Error_code error_code)
+{
+    if(m_error_code != error_code)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+//获取错误码
+Error_code Error_manager::get_error_code()
+{
+    return m_error_code;
+}
+//获取错误等级
+Error_level Error_manager::get_error_level()
+{
+    return m_error_level;
+}
+//获取错误描述的指针，（浅拷贝）
+char* Error_manager::get_error_description()
+{
+    return pm_error_description;
+}
+
+//复制错误描述，（深拷贝）
+//output：p_error_description     错误描述的字符串指针，不可以为NULL，必须要有实际的内存
+//output:description_length      错误描述的字符串长度，不可以为0，长度最好足够大，一般256即可。
+void Error_manager::copy_error_description(const char* p_error_description, int description_length)
+{
+    if(p_error_description != NULL && pm_error_description != NULL)
+    {
+        char *pt_source = (char *)p_error_description;
+        char* pt_destination = pm_error_description;
+
+        int t_length_min = m_description_length;
+        if(m_description_length > description_length)
+        {
+            t_length_min = description_length;
+        }
+
+        for(int i=0;i<t_length_min; i++)
+        {
+            *pt_destination = *pt_source;
+            pt_destination++;
+            pt_source++;
+        }
+    }
+
+    return;
+}
+//复制错误描述，（深拷贝）
+//output：error_description_string     错误描述的string
+void Error_manager::copy_error_description(std::string & error_description_string)
+{
+    if( (!error_description_string.empty() ) && pm_error_description != NULL)
+    {
+        error_description_string = pm_error_description;
+    }
+    return;
+}
+
+//设置错误码
+void Error_manager::set_error_code(Error_code error_code)
+{
+    m_error_code = error_code;
+    return;
+}
+//比较错误等级并升级，取高等级的结果
+void Error_manager::set_error_level_up(Error_level error_level)
+{
+    if(m_error_level < error_level)
+    {
+        m_error_level = error_level;
+    }
+    return;
+}
+//比较错误等级并降级，取低等级的结果
+void Error_manager::set_error_level_down(Error_level error_level)
+{
+    if(m_error_level > error_level)
+    {
+        m_error_level = error_level;
+    }
+    return;
+}
+//错误等级，设定到固定值
+void Error_manager::set_error_level_location(Error_level error_level)
+{
+    m_error_level = error_level;
+    return;
+}
+//设置错误描述
+void Error_manager::set_error_description(const char* p_error_description, int description_length)
+{
+    reallocate_memory_and_copy_string(p_error_description, description_length);
+    return ;
+}
+//设置错误描述
+void Error_manager::set_error_description(std::string & error_description_string)
+{
+    reallocate_memory_and_copy_string(error_description_string);
+    return ;
+}
+
+//尾部追加错误描述
+void Error_manager::add_error_description(const char* p_error_description, int description_length)
+{
+    if(p_error_description !=NULL)
+    {
+        char* pt_description_front = pm_error_description;
+        int t_description_front_length = m_description_length;
+        char* pt_description_back = (char *)p_error_description;
+        int t_description_back_length = 0;
+
+        if(description_length == 0)
+        {
+            t_description_back_length = 0;
+            while (*pt_description_back != '\0')
+            {
+                t_description_back_length++;
+                pt_description_back++;
+            }
+            t_description_back_length++;
+            pt_description_back = (char *)p_error_description;
+        }
+        else
+        {
+            t_description_back_length = description_length;
+        }
+
+        int t_description_new_length = t_description_front_length + 5 + t_description_back_length - 1;
+        char* pt_description_new =  (char*) malloc(t_description_new_length );
+
+        sprintf(pt_description_new, "%s ### %s", pt_description_front, pt_description_back);
+        free_description();
+        pm_error_description = pt_description_new;
+        m_description_length = t_description_new_length;
+    }
+    return ;
+}
+//尾部追加错误描述
+void Error_manager::add_error_description(std::string & error_description_string)
+{
+    if( ! error_description_string.empty() )
+    {
+        std::string t_description_string = pm_error_description ;
+        t_description_string += (" ### "+ error_description_string);
+
+        reallocate_memory_and_copy_string(t_description_string);
+    }
+}
+
+//比较错误是否相同，
+// 注：只比较错误码和等级
+bool Error_manager::is_equal_error_manager(const Error_manager & error_manager)
+{
+    if(this->m_error_code == error_manager.m_error_code
+       && this->m_error_level == error_manager.m_error_level)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+//比较并覆盖错误，讲低级错误转为字符串存放于描述中，
+//如果错误相同，则保留this的，将输入参数转入描述。
+void Error_manager::compare_and_cover_error(const Error_manager & error_manager)
+{
+    if(this->m_error_code == SUCCESS)
+    {
+        error_manager_reset(error_manager);
+    }
+    else if (error_manager.m_error_code == SUCCESS)
+    {
+		return;
+    }
+    else
+    {
+        Error_manager t_error_manager_new;
+        char* pt_string_inside = NULL;
+        int t_string_inside_length = 0;
+        if(this->m_error_level < error_manager.m_error_level)
+        {
+            t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + this->m_description_length;
+            pt_string_inside = (char*)malloc(t_string_inside_length);
+            translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+            error_manager_reset(error_manager);
+            add_error_description(pt_string_inside,t_string_inside_length);
+        }
+        else
+        {
+            t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + error_manager.m_description_length;
+            pt_string_inside = (char*)malloc(t_string_inside_length);
+			((Error_manager & )error_manager).translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+            add_error_description(pt_string_inside,t_string_inside_length);
+        }
+    }
+}
+//比较并覆盖错误，讲低级错误转为字符串存放于描述中，
+//如果错误相同，则保留this的，将输入参数转入描述。
+void Error_manager::compare_and_cover_error( Error_manager * p_error_manager)
+{
+	if(this->m_error_code == SUCCESS)
+	{
+		error_manager_reset(*p_error_manager);
+	}
+	else if (p_error_manager->m_error_code == SUCCESS)
+	{
+		//
+	}
+	else
+	{
+		Error_manager t_error_manager_new;
+		char* pt_string_inside = NULL;
+		int t_string_inside_length = 0;
+		if(this->m_error_level < p_error_manager->m_error_level)
+		{
+			t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + this->m_description_length;
+			pt_string_inside = (char*)malloc(t_string_inside_length);
+			translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+			error_manager_reset(*p_error_manager);
+			add_error_description(pt_string_inside,t_string_inside_length);
+		}
+		else
+		{
+			t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + p_error_manager->m_description_length;
+			pt_string_inside = (char*)malloc(t_string_inside_length);
+			p_error_manager->translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+			add_error_description(pt_string_inside,t_string_inside_length);
+		}
+	}
+}
+
+//将所有的错误信息，格式化为字符串，用作日志打印。
+//output：p_error_description     错误汇总的字符串指针，不可以为NULL，必须要有实际的内存
+//output:description_length      错误汇总的字符串长度，不可以为0，长度最好足够大，一般256即可。
+void Error_manager::translate_error_to_string(char* p_error_aggregate, int aggregate_length )
+{
+    char t_string_array[ERROR_NAMAGER_TO_STRING_FRONT_LENGTH] = {0};
+    char* pt_index_front = t_string_array;
+    char* pt_index_back = pm_error_description;
+    char* pt_index = p_error_aggregate;
+
+    sprintf(t_string_array, "error_code:0x%08x, error_level:%02d, error_description:", m_error_code , m_error_level );
+
+    int t_length_min = m_description_length + ERROR_NAMAGER_TO_STRING_FRONT_LENGTH -1;
+    if(t_length_min > aggregate_length)
+    {
+        t_length_min = aggregate_length;
+    }
+
+    for(int i=0;i<t_length_min; i++)
+    {
+        if(i < ERROR_NAMAGER_TO_STRING_FRONT_LENGTH -1)
+        {
+            *pt_index = *pt_index_front;
+            pt_index++;
+            pt_index_front++;
+        }
+        else
+        {
+            *pt_index = *pt_index_back;
+            pt_index++;
+            pt_index_back++;
+        }
+    }
+}
+//output：error_description_string     错误汇总的string
+void Error_manager::translate_error_to_string(std::string & error_aggregate_string)
+{
+    char t_string_array[ERROR_NAMAGER_TO_STRING_FRONT_LENGTH] = {0};
+    sprintf(t_string_array, "error_code:0x%08x, error_level:%02d, error_description:", m_error_code , m_error_level );
+
+    error_aggregate_string = t_string_array ;
+    if(pm_error_description != NULL)
+    {
+        error_aggregate_string += pm_error_description;
+    }
+    else
+    {
+        //error_aggregate_string += "NULL";
+    }
+}
+//错误码转字符串的简易版，可支持cout<<
+//return     错误汇总的string
+std::string Error_manager::to_string()
+{
+    std::string t_string;
+    translate_error_to_string(t_string);
+    return t_string;
+}
+
+
+
+
+//释放错误描述的内存，
+void Error_manager::free_description()
+{
+    if(pm_error_description != NULL)
+    {
+        free (pm_error_description);
+        pm_error_description = NULL;
+    }
+    m_description_length = 0;
+}
+
+//重新分配错误描述的内存，并从外部拷贝新的（深拷贝）
+//input：p_error_description     错误描述的字符串指针，可以为NULL，
+//input:description_length      错误描述的字符串长度，如果为0，则从p_error_description里面获取有效的长度
+void Error_manager::reallocate_memory_and_copy_string(const char* p_error_description, int description_length)
+{
+    free_description();
+
+    if(p_error_description != NULL)
+    {
+        char* pt_source = (char *)p_error_description;
+        char* pt_destination = NULL;
+
+        if(description_length == 0)
+        {
+            m_description_length = 0;
+            while (*pt_source != '\0')
+            {
+                m_description_length++;
+                pt_source++;
+            }
+            m_description_length++;
+            pt_source = (char *)p_error_description;
+        }
+        else
+        {
+            m_description_length = description_length;
+        }
+
+        pm_error_description =  (char*) malloc(m_description_length );
+        pt_destination = pm_error_description;
+
+        for(int i=0;i<m_description_length; i++)
+        {
+            *pt_destination = *pt_source;
+            pt_destination++;
+            pt_source++;
+        }
+    }
+
+    return;
+}
+
+
+//重新分配错误描述的内存，并从外部拷贝新的（深拷贝）
+//input：error_aggregate_string     错误描述的string
+void Error_manager::reallocate_memory_and_copy_string(std::string & error_aggregate_string)
+{
+    free_description();
+
+    if( ! error_aggregate_string.empty())
+    {
+        m_description_length = error_aggregate_string.length()+1;
+
+        pm_error_description =  (char*) malloc( m_description_length );
+
+        strcpy(pm_error_description ,   error_aggregate_string.c_str()  );
+    }
+}
+
+
+
+
+

src/robot_control/src/cifx/error_code.h

@@ -0,0 +1,396 @@
+
+//Error_code是错误码的底层通用模块，
+//功能：用作故障分析和处理。
+
+//用法：所有的功能接口函数return错误管理类，
+//然后上层判断分析错误码，并进行故障处理。
+
+
+
+#ifndef TEST_ERROR_ERROR_CODE_H
+#define TEST_ERROR_ERROR_CODE_H
+
+#include <string>
+#include <string.h>
+
+//错误管理类转化为字符串 的前缀，固定长度为58
+//这个是由显示格式来确定的，如果要修改格式或者 Error_code长度超过8位，Error_level长度超过2位，折需要重新计算
+#define ERROR_NAMAGER_TO_STRING_FRONT_LENGTH   58
+
+//进程加锁的状态，
+enum Lock_status
+{
+    UNLOCK      = 0,
+    LOCK        = 1,
+};
+
+//设备使能状态，
+enum Able_status
+{
+    UNABLE      = 0,
+    ENABLE      = 1,
+};
+
+//数据是否为空
+enum Empty_status
+{
+    NON_EMPTY   = 0,
+    EMPTY       = 1,
+};
+
+
+//错误码的枚举，用来做故障分析
+enum Error_code
+{
+    //成功，没有错误，默认值0
+    SUCCESS                         = 0x00000000,
+
+
+    //基本错误码，
+    ERROR                           = 0x00000001,//错误
+    PARTIAL_SUCCESS                 = 0x00000002,//部分成功
+    WARNING                         = 0x00000003,//警告
+    FAILED                          = 0x00000004,//失败
+
+    NODATA                          = 0x00000010,//没有数据，传入参数容器内部没有数据时，
+
+    POINTER_IS_NULL                 = 0x00000101,//空指针
+    PARAMETER_ERROR                 = 0x00000102,//参数错误，传入参数不符合规范时，
+    POINTER_MALLOC_FAIL             = 0x00000103,//手动分配内存失败
+
+    CLASS_BASE_FUNCTION_CANNOT_USE  = 0x00000201,//基类函数不允许使用，必须使用子类的
+
+
+//    错误码的规范，
+//    错误码是int型，32位，十六进制。
+//    例如0x12345678
+//    12表示功能模块，例如:laser雷达模块               框架制定
+//    34表示文件名称，例如:laser_livox.cpp             框架制定
+//    56表示具体的类，例如:class laser_livox           个人制定
+//    78表示类的函数，例如:laser_livox::start();       个人制定
+//    注：错误码的制定从1开始，不要从0开始，
+//        0用作错误码的基数，用来位运算，来判断错误码的范围。
+
+//    laser扫描模块
+    LASER_ERROR_BASE                = 0x01000000,
+
+//    laser_base基类
+	LASER_BASE_ERROR_BASE			= 0x01010000,
+    LASER_TASK_PARAMETER_ERROR      = 0x01010001,   //雷达任务输入参数错误
+    LASER_CONNECT_FAILED,
+	LASER_START_FAILED,
+	LASER_CHECK_FAILED				,
+	LASER_STATUS_ERROR,								//雷达状态错误
+
+
+    LASER_LIVOX_SKD_INIT_FAILED,
+
+
+//    laser_livox.cpp的错误码
+    LIVOX_ERROR_BASE                = 0x01020000,
+    LIVOX_START_FAILE               = 0x01020101,
+	LIVOX_TASK_TYPE_ERROR,							//livox任务类型错误
+
+
+     //PLC error code  ...
+    PLC_ERROR_BASE                  = 0x02010000,
+    PLC_UNKNOWN_ERROR,
+    PLC_EMPTY_TASK,
+    PLC_IP_PORT_ERROR,
+    PLC_SLAVE_ID_ERROR,
+    PLC_CONNECTION_FAILED,
+    PLC_READ_FAILED,
+    PLC_WRITE_FAILED,
+    PLC_NOT_ENOUGH_DATA_ERROR,
+
+    //Locater.cpp error from 0x0301000-0x030100FF
+        LOCATER_TASK_INIT_CLOUD_EMPTY=0x03010000,
+    LOCATER_TASK_ERROR,
+    LOCATER_TASK_INPUT_CLOUD_UNINIT,
+    LOCATER_INPUT_CLOUD_EMPTY,
+    LOCATER_YOLO_UNINIT,
+    LOCATER_POINTSIFT_UNINIT,
+    LOCATER_3DCNN_UNINIT,
+    LOCATER_INPUT_YOLO_CLOUD_EMPTY,
+    LOCATER_Y_OUT_RANGE_BY_PLC,
+    LOCATER_MEASURE_HEIGHT_CLOUD_UNINIT,
+    LOCATER_MEASURE_HEIGHT_CLOUD_EMPTY,
+    LOCATER_INPUT_CLOUD_UNINIT,
+
+
+    //point sift from 0x03010100-0x030101FF
+        LOCATER_SIFT_INIT_FAILED=0x03010100,
+    LOCATER_SIFT_INPUT_CLOUD_UNINIT,
+    LOCATER_SIFT_PREDICT_FAILED,
+    LOCATER_SIFT_CREATE_INPUT_DATA_FAILED,
+    LOCATER_SIFT_FILTE_OBS_FAILED,
+    LOCATER_SIFT_INPUT_BOX_PARAMETER_FAILED,
+    LOCATER_SIFT_INPUT_CLOUD_EMPTY,
+    LOCATER_SIFT_PREDICT_NO_CAR_POINT,
+
+    //yolo from 0x03010200-0x030102FF
+        LOCATER_YOLO_DETECT_FAILED=0x03010200,
+    LOCATER_YOLO_DETECT_NO_TARGET,
+    LOCATER_YOLO_PARAMETER_INVALID,
+    LOCATER_YOLO_INPUT_CLOUD_UNINIT,
+
+    //3dcnn from 0x03010300-0x030103FF
+    LOCATER_3DCNN_INIT_FAILED=0x03010300,
+    LOCATER_3DCNN_INPUT_CLOUD_UNINIT,
+    LOCATER_3DCNN_PREDICT_FAILED,
+    LOCATER_3DCNN_VERIFY_RECT_FAILED_3,
+    LOCATER_3DCNN_VERIFY_RECT_FAILED_4,
+    LOCATER_3DCNN_KMEANS_FAILED,
+    LOCATER_3DCNN_IIU_FAILED,
+    LOCATER_3DCNN_INPUT_CLOUD_EMPTY,
+    LOCATER_3DCNN_PCA_OUT_CLOUD_EMPTY,
+
+    //System_manager error from 0x04010000-0x0401FFFF
+    SYSTEM_READ_PARAMETER_ERROR=0x04010100,
+    SYSTEM_PARAMETER_ERROR,
+    SYSTEM_INPUT_TERMINOR_NO_LASERS,
+
+    //terminor_command_executor.cpp from 0x04010200-0x040102FF
+    TERMINOR_NOT_READY=0x04010200,
+    TERMINOR_INPUT_LASER_NULL,
+    TERMINOR_NOT_CONTAINS_LASER,
+    TERMINOR_INPUT_PLC_NULL,
+    TERMINOR_INPUT_LOCATER_NULL,
+    TERMINOR_CREATE_WORKING_THREAD_FAILED,
+    TERMINOR_FORCE_QUIT,
+    TERMINOR_LASER_TIMEOUT,
+    TERMINOR_POST_PLC_TIMEOUT,
+    TERMINOR_CHECK_RESULTS_ERROR,
+
+    ////Hardware limit from 0x05010000 - 0x0501ffff
+    ///railing.cpp from 0x05010100-0x050101ff
+    HARDWARE_LIMIT_LEFT_RAILING=0x05010100,         //左栏杆限制
+    HARDWARE_LIMIT_RAILING_PARAMETER_ERROR,
+    HARDWARE_LIMIT_RAILING_ERROR,
+    HARDWARE_LIMIT_CENTER_X_LEFT,
+    HARDWARE_LIMIT_CENTER_X_RIGHT,
+    HARDWARE_LIMIT_CENTER_Y_TOP,
+    HARDWARE_LIMIT_CENTER_Y_BOTTOM,
+    HARDWARE_LIMIT_HEIGHT_OUT_RANGE,
+    HARDWARE_LIMIT_ANGLE_OUT_RANGE,
+    //termonal_limit from 0x05010200-0x050102ff
+    HARDWARE_LIMIT_TERMINAL_LEFT_ERROR,
+    HARDWARE_LIMIT_TERMINAL_RIGHT_ERROR,
+    HARDWARE_LIMIT_TERMINAL_LR_ERROR,
+
+
+    //wj_lidar error from 0x06010000-0x0601FFFF
+        WJ_LIDAR_CONNECT_FAILED=0x06010000,
+    WJ_LIDAR_UNINITIALIZED,
+    WJ_LIDAR_READ_FAILED,
+    WJ_LIDAR_WRITE_FAILED,
+    WJ_LIDAR_GET_CLOUD_TIMEOUT,
+
+    //wj lidar protocol error from 0x06020000-0x0602FFFF
+        WJ_PROTOCOL_ERROR_BASE=0x06020000,
+    WJ_PROTOCOL_INTEGRITY_ERROR,
+    WJ_PROTOCOL_PARSE_FAILED,
+    WJ_PROTOCOL_EMPTY_PACKAGE,
+    WJ_PROTOCOL_EXCEED_MAX_SIZE,
+
+    //wj region detect error from 0x06030000-0x0603FFFF
+        WJ_REGION_EMPTY_CLOUD=0x06030000,
+    WJ_REGION_RECTANGLE_ANGLE_ERROR,
+    WJ_REGION_RECTANGLE_SIZE_ERROR,
+    WJ_REGION_RECTANGLE_SYMMETRY_ERROR,
+    WJ_REGION_CLUSTER_SIZE_ERROR,
+
+    //wj manager error from 0x06040000-0x0604FFFF
+    WJ_MANAGER_UNINITIALIZED=0x06040000,
+    WJ_MANAGER_LIDAR_DISCONNECTED,
+    WJ_MANAGER_PLC_DISCONNECTED,
+    WJ_MANAGER_EMPTY_CLOUD,
+
+    WJ_LIDAR_TASK_EMPTY_RESULT=0x06050000,
+    WJ_LIDAR_TASK_EMPTY_TASK,
+    WJ_LIDAR_TASK_WRONG_TYPE,
+    WJ_LIDAR_TASK_INVALID_TASK,
+    WJ_LIDAR_TASK_MEASURE_FAILED,
+
+    //cifx error from 0x07010000-0x0701FFFF
+    CIFX_DRIVER_INIT_ERROR=0x07010000,
+    CIFX_DRIVER_OPEN_ERROR,
+    CIFX_CHANNEL_OPEN_ERROR,
+    CIFX_BUS_OPEN_ERROR,
+    CIFX_READ_FAILED,
+    CIFX_READ_TIMEOUT,
+    CIFX_WRITE_FAILED,
+    CIFX_WRITE_TIMEOUT,
+
+    //odom error from 0x07020000-0x0702FFFF
+    
+};
+
+//错误等级，用来做故障处理
+enum Error_level
+{
+//    正常，没有错误，默认值0
+    NORMAL                = 0,
+
+
+//    可忽略的故障，NEGLIGIBLE_ERROR
+//    提示作用，不做任何处理，不影响代码的流程，
+//    用作一些不重要的事件，一半出错也不会影响到系统功能，
+//    例如：文件保存错误，等
+    NEGLIGIBLE_ERROR      = 1,
+
+
+//    一般故障，MINOR_ERROR
+//    用作底层功能函数的错误返回，表示该功能函数执行失败，
+//    返回给应用层之后，需要做故障分析和处理，
+//    例如：雷达数据传输失败，应用层进行重新扫描，或者重连，或者重置参数等。
+    MINOR_ERROR           = 2,
+
+
+//    严重故障，MAJOR_ERROR
+//    用作应用层的任务事件的结果，表示该功能模块失败。
+//    通常是底层函数返回一般故障之后，应用层无法处理并解决故障，此时就要进行故障升级，
+//    从一般故障升级为严重故障，然后进行回退流程，回退已经执行的操作，最终回到故障待机状态。
+//    需要外部清除故障，并复位至正常待机状态，才能恢复功能的使用。
+//    例如：雷达扫描任务失败，且无法自动恢复。
+    MAJOR_ERROR           = 3,
+
+
+//    致命故障，CRITICAL_ERROR
+//    系统出现致命错误。导致系统无法正常运行，
+//    此时系统应该紧急停机，执行紧急流程，快速停机。
+//    此时不允许再执行任何函数和任务指令，防止系统故障更加严重。
+//    也不需要做任何错误处理了，快速执行紧急流程。
+//    例如：内存错误，进程挂死，关键设备失控，监控设备报警，等
+    CRITICAL_ERROR        = 4,
+};
+
+
+class Error_manager
+{
+public://外部接口函数
+    //构造函数
+    Error_manager();
+    //拷贝构造
+    Error_manager(const Error_manager & error_manager);
+    //赋值构造
+    Error_manager(Error_code error_code, Error_level error_level = NORMAL,
+                  const char* p_error_description = NULL, int description_length = 0);
+    //赋值构造
+    Error_manager(Error_code error_code, Error_level error_level , std::string & error_aggregate_string);
+    //析构函数
+    ~Error_manager();
+
+    //初始化
+    void error_manager_init();
+    //初始化
+    void error_manager_init(Error_code error_code, Error_level error_level = NORMAL,
+                            const char* p_error_description = NULL, int description_length = 0);
+    //初始化
+    void error_manager_init(Error_code error_code, Error_level error_level , std::string & error_aggregate_string);
+    //重置
+    void error_manager_reset(Error_code error_code, Error_level error_level = NORMAL,
+                             const char* p_error_description = NULL, int description_length = 0);
+    //重置
+    void error_manager_reset(Error_code error_code, Error_level error_level , std::string & error_aggregate_string);
+    //重置
+    void error_manager_reset(const Error_manager & error_manager);
+    //清除所有内容
+    void error_manager_clear_all();
+
+    //重载=
+    Error_manager& operator=(const Error_manager & error_manager);
+    //重载=，支持Error_manager和Error_code的直接转化，会清空错误等级和描述
+    Error_manager& operator=(Error_code error_code);
+    //重载==
+    bool operator==(const Error_manager & error_manager);
+    //重载==，支持Error_manager和Error_code的直接比较
+    bool operator==(Error_code error_code);
+    //重载!=
+    bool operator!=(const Error_manager & error_manager);
+    //重载!=，支持Error_manager和Error_code的直接比较
+    bool operator!=(Error_code error_code);
+
+
+    //获取错误码
+    Error_code get_error_code();
+    //获取错误等级
+    Error_level get_error_level();
+    //获取错误描述的指针，（浅拷贝）
+    char* get_error_description();
+
+    //复制错误描述，（深拷贝）
+    //output：p_error_description     错误描述的字符串指针，不可以为NULL，必须要有实际的内存
+    //output:description_length      错误描述的字符串长度，不可以为0，长度最好足够大，一般256即可。
+    void copy_error_description(const char* p_error_description, int description_length);
+    //复制错误描述，（深拷贝）
+    //output：error_description_string     错误描述的string
+    void copy_error_description(std::string & error_description_string);
+
+    //设置错误码
+    void set_error_code(Error_code error_code);
+    //比较错误等级并升级，取高等级的结果
+    void set_error_level_up(Error_level error_level);
+    //比较错误等级并降级，取低等级的结果
+    void set_error_level_down(Error_level error_level);
+    //错误等级，设定到固定值
+    void set_error_level_location(Error_level error_level);
+    //设置错误描述
+    void set_error_description(const char* p_error_description, int description_length = 0);
+    //设置错误描述
+    void set_error_description(std::string & error_description_string);
+
+    //尾部追加错误描述
+    void add_error_description(const char* p_error_description, int description_length = 0);
+    //尾部追加错误描述
+    void add_error_description(std::string & error_description_string);
+
+    //比较错误是否相同，
+    // 注：只比较错误码和等级
+	bool is_equal_error_manager(const Error_manager & error_manager);
+	//比较并覆盖错误，讲低级错误转为字符串存放于描述中，
+	//如果错误相同，则保留this的，将输入参数转入描述。
+	void compare_and_cover_error(const Error_manager & error_manager);
+	//比较并覆盖错误，讲低级错误转为字符串存放于描述中，
+	//如果错误相同，则保留this的，将输入参数转入描述。
+	void compare_and_cover_error( Error_manager * p_error_manager);
+
+	//将所有的错误信息，格式化为字符串，用作日志打印。
+    //output：p_error_description     错误汇总的字符串指针，不可以为NULL，必须要有实际的内存
+    //output:description_length      错误汇总的字符串长度，不可以为0，长度最好足够大，一般256即可。
+    void translate_error_to_string(char* p_error_aggregate, int aggregate_length);
+    //output：error_description_string     错误汇总的string
+    void translate_error_to_string(std::string & error_aggregate_string);
+    //错误码转字符串的简易版，可支持cout<<
+    //return     错误汇总的string
+    std::string to_string();
+
+
+
+protected:
+    Error_code              m_error_code;               //错误码
+    Error_level             m_error_level;              //错误等级
+    char*                   pm_error_description;       //错误描述
+    int                     m_description_length;       //错误描述的字符长度
+
+protected://内部功能函数
+public:
+    //释放错误描述的内存，
+    void free_description();
+
+    //重新分配错误描述的内存，并从外部拷贝新的（深拷贝）
+    //input：p_error_description     错误描述的字符串指针，可以为NULL，
+    //input:description_length      错误描述的字符串长度，如果为0，则从p_error_description里面获取有效的长度
+    void reallocate_memory_and_copy_string(const char* p_error_description, int description_length = 0);
+
+    //重新分配错误描述的内存，并从外部拷贝新的（深拷贝）
+    //input：error_aggregate_string     错误描述的string
+    void reallocate_memory_and_copy_string(std::string & error_aggregate_string);
+};
+
+
+
+
+#endif //TEST_ERROR_ERROR_CODE_H
+
+

src/robot_control/src/cp1616/CP16Card.cpp

@@ -0,0 +1,165 @@
+//
+// Created by zx on 2019/8/19.
+//
+
+#include "CP16Card.h"
+#include <stdio.h>
+#include <string.h>
+#include <pniobase.h>
+
+bool CP16Card::m_quit=false;
+device_data_t CP16Card::m_write_data=device_data_t();
+device_data_t CP16Card::m_read_data=device_data_t();
+Semaphore     g_read_signal;
+Semaphore     g_write_signal;
+CP16Card::CP16Card()
+{}
+CP16Card::~CP16Card()
+{}
+
+
+
+bool CP16Card::Init()
+{
+    static PNIOD_CBF_FUNCTIONS callbacks;
+    memset(&callbacks,0,sizeof(callbacks));
+    callbacks.size = sizeof(callbacks);
+    callbacks.cbf_data_write = PNIO_cbf_data_write;
+    callbacks.cbf_data_read = PNIO_cbf_data_read;
+
+    m_write_data.api = 0;
+    m_write_data.InDatLength = 4; /* convert device-view to controller-view */
+    m_write_data.OutDatLength = 0; /* convert device-view to controller-view */
+    m_write_data.OutData = 0;
+    m_write_data.OutDatIocs = PNIO_S_BAD;
+    m_write_data.OutDatIops = PNIO_S_BAD;
+    m_write_data.InData = (PNIO_UINT8*)malloc(sizeof(PNIO_UINT8) * 4);
+    m_write_data.InDatIops = PNIO_S_BAD;
+    m_write_data.InDatIocs = PNIO_S_BAD;
+
+
+    m_read_data.api = 0;
+    m_read_data.InDatLength = 0; /* convert device-view to controller-view */
+    m_read_data.OutDatLength = 4; /* convert device-view to controller-view */
+    m_read_data.OutData = (PNIO_UINT8*)malloc(sizeof(PNIO_UINT8) * 4);
+    m_read_data.OutDatIocs = PNIO_S_BAD;
+    m_read_data.OutDatIops = PNIO_S_BAD;
+    m_read_data.InData = 0;
+    m_read_data.InDatIops = PNIO_S_BAD;
+    m_read_data.InDatIocs = PNIO_S_BAD;
+
+
+    InitCard(&callbacks);
+    //开启线程，读取数据
+
+}
+
+bool CP16Card::UnInit()
+{
+    UnInitCard();
+    return true;
+}
+
+
+float reserveByte(float data)
+{
+    int size=sizeof(float);
+    float ret;
+    char* p=(char*)&ret;
+    char* p2=(char*)&data;
+    for(int i=0;i<size;++i)
+        p[size-1-i]=p2[i];
+    return ret;
+}
+
+
+float CP16Card::ReadData(PNIO_UINT32 submodel)
+{
+    static PNIO_DEV_ADDR addr;
+    addr.AddrType=PNIO_ADDR_GEO;
+    addr.IODataType=PNIO_IO_IN_OUT;
+    addr.u.Addr=0;
+    addr.u.Geo.Slot=1;
+    addr.u.Geo.Subslot=submodel;
+    Trigger_data_read(&addr);
+    g_read_signal.Wait();
+
+    float ret=0;
+    memcpy(&ret,m_read_data.OutData,sizeof(float));
+    float data=reserveByte(ret);
+    return data;
+
+}
+
+void CP16Card::WriteData(PNIO_UINT32 submodel, float data)
+{
+    static PNIO_DEV_ADDR addr;
+    addr.AddrType=PNIO_ADDR_GEO;
+    addr.IODataType=PNIO_IO_IN_OUT;
+    addr.u.Addr=0;
+    addr.u.Geo.Slot=1;
+    addr.u.Geo.Subslot=submodel;
+
+    float wd=reserveByte(data);
+    memcpy(m_write_data.InData,&wd,sizeof(float));
+    Trigger_data_write(&addr);
+    g_write_signal.Wait();
+
+
+}
+
+PNIO_IOXS CP16Card::PNIO_cbf_data_read(PNIO_UINT32 DevHndl,
+                                       PNIO_DEV_ADDR *pAddr,
+                                       PNIO_UINT32 BufLen,
+                                       PNIO_UINT8 *pBuffer,
+                                       PNIO_IOXS Iops)
+{
+    if (BufLen == 0)
+    {
+        printf(" BufLen = 0, nothing to read..\n");
+        m_read_data.OutDatIocs = PNIO_S_GOOD;
+    }
+    else if (BufLen <= (PNIO_UINT32)m_read_data.OutDatLength)
+    {
+        memset(m_read_data.OutData, 0, m_read_data.OutDatLength);
+        memcpy(m_read_data.OutData, pBuffer, BufLen);  /*Copy the data from the stack to the application buffer*/
+        m_read_data.OutDatIocs = PNIO_S_GOOD; /* assume everything is ok */
+
+        //printf("  read data length %d  submodel %d  data : <----- \n",BufLen,pAddr->u.Geo.Subslot);
+    }
+    else
+    {
+        printf("NEW PNIO_cbf_data_read: Buflen=%lu > allowed size (%u)!!! Abort reading...  IOCS=  BAD !!! \n",
+               (unsigned long)BufLen, m_read_data.OutDatLength);
+    }
+    g_read_signal.Signal();
+    return (m_read_data.OutDatIocs); /* consumer state (of local IO device) */
+}
+
+PNIO_IOXS CP16Card::PNIO_cbf_data_write(PNIO_UINT32 DevHndl,
+                                        PNIO_DEV_ADDR *pAddr,
+                                        PNIO_UINT32 BufLen,
+                                        PNIO_UINT8 *pBuffer,
+                                        PNIO_IOXS Iocs)
+{
+
+    m_write_data.InDatIocs = Iocs;
+    m_write_data.InDatIops = PNIO_S_BAD;
+    if (BufLen == 0)
+    {
+        m_write_data.InDatIops = PNIO_S_GOOD;
+    }
+    else if (BufLen <= (PNIO_UINT32)m_write_data.InDatLength)
+    {
+        memcpy(pBuffer, m_write_data.InData, BufLen);
+        m_write_data.InDatIops = PNIO_S_GOOD;
+
+    }
+    else
+    {
+        printf("!!! PNIO_cbf_data_write: Buflen=%lu > allowed size (%u)!!! Abort writing..\n",
+               (unsigned long)BufLen, m_write_data.InDatLength);
+    }
+    g_write_signal.Signal();
+    return m_write_data.InDatIops;
+}

src/robot_control/src/cp1616/CP16Card.h

@@ -0,0 +1,69 @@
+//
+// Created by zx on 2019/8/19.
+//
+
+#ifndef CP16CARD_H
+#define CP16CARD_H
+extern "C"
+{
+    #include "dev_certify_energy.h"
+};
+
+#include <mutex>
+#include <condition_variable>
+class Semaphore {
+public:
+    explicit Semaphore() : count_(0) {
+    }
+
+    void Signal() {
+        std::unique_lock<std::mutex> lock(mutex_);
+        ++count_;
+        cv_.notify_one();
+    }
+
+    void Wait() {
+        std::unique_lock<std::mutex> lock(mutex_);
+        cv_.wait(lock, [=] { return count_ > 0; });
+        --count_;
+    }
+
+private:
+    std::mutex mutex_;
+    std::condition_variable cv_;
+    int count_;
+};
+
+#include <thread>
+class CP16Card
+{
+protected:
+    static bool m_quit;
+    static device_data_t m_write_data;
+    static device_data_t m_read_data;
+public:
+    CP16Card();
+    ~CP16Card();
+    static bool Init();
+    static bool UnInit();
+    void WriteData(PNIO_UINT32 submodel,float data);
+    float ReadData(PNIO_UINT32 submodel);
+
+protected:
+    static PNIO_IOXS PNIO_cbf_data_write(
+        PNIO_UINT32    DevHndl,            /* [in] Handle for device */
+        PNIO_DEV_ADDR* pAddr,              /* [in] geographical address def. in 'pniobase.h' */
+        PNIO_UINT32    BufLen,             /* [in] length of the submodule input data */
+        PNIO_UINT8*    pBuffer,            /* [out] Ptr to data buffer to write to */
+        PNIO_IOXS      Iocs);
+
+    static PNIO_IOXS PNIO_cbf_data_read(
+        PNIO_UINT32    DevHndl,            /* [in] Handle for Multidevice */
+        PNIO_DEV_ADDR* pAddr,              /* [in] geographical address */
+        PNIO_UINT32    BufLen,             /* [in] length of the submodule input data */
+        PNIO_UINT8*    pBuffer,            /* [in] Ptr to data buffer to read from */
+        PNIO_IOXS      Iops);             /* [in] (io controller) provider status */
+};
+
+
+#endif //CP16CARD_H

src/robot_control/src/cp1616/dev_certify_energy.h

@@ -0,0 +1,148 @@
+/*------------------------------------------------------------------------*/
+/* Copyright (C) SIEMENS CORP., 2018 All rights reserved.*/
+/* All rights reserved.                                                   */
+/* Redistribution and use in source and binary forms, with or without     */
+/* modification, are permitted provided that the following conditions     */
+/* are met:                                                               */
+/* 1. Redistributions of source code must retain the above copyright      */
+/* notice, this list of conditions and the following disclaimer.          */
+/* 2. Redistributions in binary form must reproduce the above copyright   */
+/* notice, this list of conditions and the following disclaimer in the    */
+/* documentation and/or other materials provided with the distribution.   */
+/* 3. Neither the name of the copyright holder nor the names of its       */
+/* contributors may be used to endorse or promote products derived from   */
+/* this software without specific prior written permission.               */
+/* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS    */
+/* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT      */
+/* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS      */
+/* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE         */
+/* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,   */
+/* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,   */
+/* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS  */
+/* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)                    */
+/* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,    */
+/* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING  */
+/* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE     */
+/* POSSIBILITY OF SUCH DAMAGE.                                            */
+/*------------------------------------------------------------------------*/
+/*------------------------------------------------------------------------*/
+/* Description: Main module: Device init, deinit and callbacks.           */
+/*                                                                        */
+/*------------------------------------------------------------------------*/
+/* Attention : Callbacks are running concurrent in other threads so all   */
+/*             printf statements should be synchronized. But this sample  */
+/*             application doesn't synchronize for simplicity  !          */
+/*------------------------------------------------------------------------*/
+
+#ifndef _DEV_CERTIFY_ENERGY_H
+#define _DEV_CERTIFY_ENERGY_H
+
+#include "dev_cfg.h"
+#include "pniousrd.h"
+
+#define PROFI_ENERGY_ENABLE /*  enable PROFIENERGY extensions */
+#define PE_PRINTF printf
+/* #define PE_PRINTF */
+
+
+/******************************************************************************/
+/* SWAPPING of 16 and 32 Bit values depending of endianess of the Host-system */
+/* endian.                                                                    */
+/******************************************************************************/
+#define SWAP_16(var)                  \
+	(  ((( (var)) & 0xFF00L) >>  8)   \
+	+ ((( (var)) & 0x00FFL) << 8))
+
+#define SWAP_32(var)                     \
+	(  ((( (var)) & 0xFF000000L) >> 24)  \
+	+ ((( (var)) & 0x00FF0000L) >>  8)   \
+	+ ((( (var)) & 0x0000FF00L) <<  8)   \
+	+ ((( (var)) & 0x000000FFL) << 24))
+
+
+#ifdef _DPR_LINUX
+#if __BYTE_ORDER == __BIG_ENDIAN
+#define NO_SWAP
+#else
+#define SWAP
+#endif
+#else
+#define SWAP
+#endif
+
+
+#ifdef NOSWAP /* Linux Big Eandian */
+#define CPU_TO_LE16(c) SWAP_16(c)
+#define LE_TO_CPU16(c) SWAP_16(c)
+#define CPU_TO_LE(c) SWAP_32(c)
+#define LE_TO_CPU(c) SWAP_32(c)
+
+#define CPU_TO_BE16(c) (c)
+#define BE_TO_CPU16(c) (c)
+#define CPU_TO_BE(c) (c)
+#define BE_TO_CPU(c) (c)
+#define SWAP_FLOAT(a,b) 	b = a
+
+#else /* Linux Little Endian or Windows */
+#define CPU_TO_LE16(c) (c)
+#define LE_TO_CPU16(c) (c)
+#define CPU_TO_LE(c) (c)
+#define LE_TO_CPU(c) (c)
+
+#define CPU_TO_BE16(c) SWAP_16(c)
+#define BE_TO_CPU16(c) SWAP_16(c)
+#define CPU_TO_BE(c) SWAP_32(c)
+#define BE_TO_CPU(c) SWAP_32(c)
+#define SWAP_FLOAT(b,a) 		{	PNIO_UINT32 dw_Help = BE_TO_CPU(*((PNIO_UINT32*)&a)); \
+	b = (float)*((float*)&dw_Help); \
+							}
+
+#endif
+
+
+/****************************************************/
+/* Known Diagnostic alarms for this device          */
+/****************************************************/
+
+#define CH_ERR_INVAL_LINKUP     0x0100
+#define CH_ERR_INVAL_LINKDOWN   0x0101
+#define CH_ERR_NO_REDUND_PS     0x0200
+#define CH_ERR_NO_CPLUG         0x0201
+#define CH_ERR_CPLUG_ERROR      0x0202
+
+/*****************************************/
+/* help data for modules and sub modules */
+/*****************************************/
+typedef struct
+{
+	PNIO_DEV_ADDR Addr;
+	PNIO_UINT32 modId;
+	PNIO_UINT32 subslotId;
+	PNIO_UINT32 api;
+	PNIO_UINT32 InDatLength;
+	PNIO_UINT32 OutDatLength;
+	PNIO_UINT8* OutData;
+	PNIO_IOXS OutDatIocs;
+	PNIO_IOXS OutDatIops;
+	PNIO_UINT8* InData;
+	PNIO_IOXS InDatIops;
+	PNIO_IOXS InDatIocs;
+} device_data_t;
+
+typedef struct
+{
+	unsigned short VendorId;
+	int nrOfSubmodules;
+	device_data_t* pDeviceData;
+} device_data_set_t;
+
+extern device_data_set_t g_device_data;
+
+int addrToId(PNIO_DEV_ADDR addr);
+int InitCard(PNIOD_CBF_FUNCTIONS* pCallbacks);
+int UnInitCard();
+PNIO_UINT32 Trigger_data_read(PNIO_DEV_ADDR*     pAddr);
+PNIO_UINT32 Trigger_data_write(PNIO_DEV_ADDR*     pAddr);
+
+
+#endif /* _DEV_CERTIFY_ENERGY_H */

src/robot_control/src/cp1616/dev_cfg.h

@@ -0,0 +1,69 @@
+/*------------------------------------------------------------------------*/
+/* Copyright (C) SIEMENS CORP., 2018 All rights reserved.*/
+/* All rights reserved.                                                   */
+/* Redistribution and use in source and binary forms, with or without     */
+/* modification, are permitted provided that the following conditions     */
+/* are met:                                                               */
+/* 1. Redistributions of source code must retain the above copyright      */
+/* notice, this list of conditions and the following disclaimer.          */
+/* 2. Redistributions in binary form must reproduce the above copyright   */
+/* notice, this list of conditions and the following disclaimer in the    */
+/* documentation and/or other materials provided with the distribution.   */
+/* 3. Neither the name of the copyright holder nor the names of its       */
+/* contributors may be used to endorse or promote products derived from   */
+/* this software without specific prior written permission.               */
+/* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS    */
+/* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT      */
+/* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS      */
+/* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE         */
+/* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,   */
+/* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,   */
+/* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS  */
+/* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)                    */
+/* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,    */
+/* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING  */
+/* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE     */
+/* POSSIBILITY OF SUCH DAMAGE.                                            */
+/*------------------------------------------------------------------------*/
+/*------------------------------------------------------------------------*/
+/* Description: A few data for configuration of the simulated device.     */
+/*              THIS MODULE HAS TO BE MODIFIED BY THE PNIO USER.          */
+/*                                                                        */
+/*------------------------------------------------------------------------*/
+
+/***************************************************/
+/* PNIO_ANNOTATION entries                         */
+/***************************************************/
+#include <pniousrd.h>
+/* CP1626 */
+#define ANNOT_CP1626_NAME       "S7-PC"           /* device type (String 25) */
+#define ANNOT_CP1626_ORDERID    "6GK1 162-6AA01"  /* Order Id    (String 20) */
+#define ANNOT_CP1626_HW_REV     2                 /* HwRevision  (short)     */
+#define ANNOT_CP1626_SW_PREFIX  'V'               /* SwRevisionPrefix (char) */
+#define ANNOT_CP1626_SW_REV_1   1                 /* SwRevision1 (short)     */
+#define ANNOT_CP1626_SW_REV_2   0                 /* SwRevision2 (short)     */
+#define ANNOT_CP1626_SW_REV_3   0                 /* SwRevision3 (short)     */
+
+/* CP1616 */
+#define ANNOT_CP1616_NAME       "S7-PC"           /* device type (String 25) */
+#define ANNOT_CP1616_ORDERID    "6GK1 161-6AA02"  /* Order Id    (String 20) */
+#define ANNOT_CP1616_HW_REV     9                 /* HwRevision  (short)     */
+#define ANNOT_CP1616_SW_PREFIX  'V'               /* SwRevisionPrefix (char) */
+#define ANNOT_CP1616_SW_REV_1   2                 /* SwRevision1 (short)     */
+#define ANNOT_CP1616_SW_REV_2   7                 /* SwRevision2 (short)     */
+#define ANNOT_CP1616_SW_REV_3   2                 /* SwRevision3 (short)     */
+
+/* CP1604 */
+#define ANNOT_CP1604_NAME       "S7-PC"           /* device type (String 25) */
+#define ANNOT_CP1604_ORDERID    "6GK1 160-4AA01"  /* Order Id    (String 20) */
+#define ANNOT_CP1604_HW_REV     8                 /* HwRevision  (short)     */
+#define ANNOT_CP1604_SW_PREFIX  'V'               /* SwRevisionPrefix (char) */
+#define ANNOT_CP1604_SW_REV_1   2                 /* SwRevision1 (short)     */
+#define ANNOT_CP1604_SW_REV_2   7                 /* SwRevision2 (short)     */
+#define ANNOT_CP1604_SW_REV_3   2                 /* SwRevision3 (short)     */
+
+extern PNIOD_ANNOTATION g_annot_selected; /* selected Annotation */
+
+extern PNIO_UINT8*      g_annot_serial;
+
+#define MAX_SERIAL_LENGTH 16

src/robot_control/src/cp1616/ds.c

@@ -0,0 +1,267 @@
+/*------------------------------------------------------------------------*/
+/* Copyright (C) SIEMENS CORP., 2018 All rights reserved.*/
+/* All rights reserved.                                                   */
+/* Redistribution and use in source and binary forms, with or without     */
+/* modification, are permitted provided that the following conditions     */
+/* are met:                                                               */
+/* 1. Redistributions of source code must retain the above copyright      */
+/* notice, this list of conditions and the following disclaimer.          */
+/* 2. Redistributions in binary form must reproduce the above copyright   */
+/* notice, this list of conditions and the following disclaimer in the    */
+/* documentation and/or other materials provided with the distribution.   */
+/* 3. Neither the name of the copyright holder nor the names of its       */
+/* contributors may be used to endorse or promote products derived from   */
+/* this software without specific prior written permission.               */
+/* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS    */
+/* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT      */
+/* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS      */
+/* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE         */
+/* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,   */
+/* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,   */
+/* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS  */
+/* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)                    */
+/* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,    */
+/* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING  */
+/* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE     */
+/* POSSIBILITY OF SUCH DAMAGE.                                            */
+/*------------------------------------------------------------------------*/
+/*------------------------------------------------------------------------*/
+/* Attention : Callbacks are running concurrent in other threads so all   */
+/*             printf statements should be synchronized. But this sample  */
+/*             application doesn't synchronize for simplicity  !          */
+/*------------------------------------------------------------------------*/
+
+#include "ds.h"
+#include "dev_certify_energy.h"
+
+#ifdef WIN32
+#include <windows.h>
+#include <stdio.h>
+#include <conio.h>
+#else
+#define _BSD_SOURCE /* for usleep */
+#include <stdio.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <poll.h>
+#include <termios.h>
+#include <string.h>
+#include <time.h>
+#define Sleep(x)  usleep(x*1000)
+#endif
+
+/*----------------------------------------------------------------------------------------------------*/
+/*    DEFINES                                                                                         */
+/*----------------------------------------------------------------------------------------------------*/
+
+#define DS_BLOCK_TYPE_SUBST_VALUE           0x0014
+#define DS_BLOCK_TYPE_INP_DATA_OBJ_ELEM     0x0015
+#define DS_BLOCK_TYPE_OUTP_DATA_OBJ_ELEM    0x0016
+#define IEC_DS_VERSION_HIGH_1               0x01
+#define IEC_DS_VERSION_LOW_0                0x00
+#define SUBST_ACTIVE_FLAG_OFF               0x0000
+#define SUBST_ACTIVE_FLAG_ON                0x0001
+
+/*----------------------------------------------------------------------------------------------------*/
+/*    INTERNAL INTERFACE                                                                              */
+/*----------------------------------------------------------------------------------------------------*/
+
+PNIO_UINT8 IoxsIoBaseToNet(PNIO_IOXS ioxs)
+{
+	return ((ioxs == PNIO_S_GOOD) ? 0x80 : 0x00);
+}
+
+/*----------------------------------------------------------------------------------------------------*/
+/*    EXTERNAL INTERFACE                                                                              */
+/*----------------------------------------------------------------------------------------------------*/
+
+void DsInit_InputDataObject(IEC_DS_READ_INP_DATA_OBJ_ELEM* pContent, PNIO_DEV_ADDR* pAddr)
+{
+	int id;
+	PNIO_UINT16 ioDataLength;
+
+	id = addrToId(*pAddr);
+	if (id < 0)
+	{
+		return;
+	}
+
+	ioDataLength = g_device_data.pDeviceData[id].InDatLength;
+	/* block header */
+	pContent->dsBlockType = SWAP_16(DS_BLOCK_TYPE_INP_DATA_OBJ_ELEM);
+	pContent->dsBlockLength =
+		sizeof(pContent->dsBlockVersionHi)
+		+ sizeof(pContent->dsBlockVersionLo)
+		+ sizeof(pContent->lengthIocs)
+		+ sizeof(pContent->iocs)
+		+ sizeof(pContent->lengthIops)
+		+ sizeof(pContent->iops)
+		+ sizeof(pContent->lengthData)
+		+ ioDataLength;
+	pContent->dsBlockLength = SWAP_16(pContent->dsBlockLength);
+	pContent->dsBlockVersionHi = IEC_DS_VERSION_HIGH_1;
+	pContent->dsBlockVersionLo = IEC_DS_VERSION_LOW_0;
+	/* block body */
+	pContent->lengthIocs = sizeof(PNIO_UINT8);
+	pContent->iocs = IoxsIoBaseToNet(g_device_data.pDeviceData[id].InDatIocs);
+	pContent->lengthIops = sizeof(PNIO_UINT8);
+	pContent->iops = IoxsIoBaseToNet(g_device_data.pDeviceData[id].InDatIops);
+	pContent->lengthData = SWAP_16(ioDataLength);
+	pContent->pData = g_device_data.pDeviceData[id].InData;
+}
+
+void DsInit_OutputDataObject(IEC_DS_READ_OUTP_DATA_OBJ_ELEM* pContent, PNIO_DEV_ADDR* pAddr)
+{
+	int id;
+	PNIO_UINT16 ioDataLength;
+
+	id = addrToId(*pAddr);
+	if (id < 0)
+	{
+		return;
+	}
+
+	ioDataLength = g_device_data.pDeviceData[id].OutDatLength;
+	/* block length - no swap to network format here */
+	pContent->dsBlockLengthSubst =
+		sizeof(pContent->dsBlockVersionHiSubst)
+		+ sizeof(pContent->dsBlockVersionLoSubst)
+		+ sizeof(pContent->substMode)
+		+ sizeof(pContent->substIocs)
+		+ ioDataLength
+		+ sizeof(pContent->substDataValid);
+	pContent->dsBlockLength =
+		sizeof(pContent->dsBlockVersionHi)
+		+ sizeof(pContent->dsBlockVersionLo)
+		+ sizeof(pContent->substActiveFlag)
+		+ sizeof(pContent->lengthIocs)
+		+ sizeof(pContent->lengthIops)
+		+ sizeof(pContent->lengthData)
+		+ sizeof(pContent->iocs)
+		+ ioDataLength
+		+ sizeof(pContent->iops)
+		+ sizeof(pContent->dsBlockTypeSubst)
+		+ sizeof(pContent->dsBlockLengthSubst)
+		+ pContent->dsBlockLengthSubst;
+	/* block header, swap to network format */
+	pContent->dsBlockType = SWAP_16(DS_BLOCK_TYPE_OUTP_DATA_OBJ_ELEM);
+	pContent->dsBlockLength = SWAP_16(pContent->dsBlockLength);
+	pContent->dsBlockVersionHi = IEC_DS_VERSION_HIGH_1;
+	pContent->dsBlockVersionLo = IEC_DS_VERSION_LOW_0;
+	/* block body */
+
+	if (g_device_data.pDeviceData[id].OutDatIops == PNIO_S_BAD)
+	{
+		pContent->substActiveFlag = SWAP_16(SUBST_ACTIVE_FLAG_ON);
+		pContent->substDataValid = 0x80; /* good */
+	}
+	else
+	{
+		pContent->substActiveFlag = SWAP_16(SUBST_ACTIVE_FLAG_OFF);
+		pContent->substDataValid = 0x40; /* bad */
+	}
+	pContent->lengthIocs = sizeof(PNIO_UINT8);
+	pContent->lengthIops = sizeof(PNIO_UINT8);
+	pContent->lengthData = SWAP_16(ioDataLength);
+	pContent->iocs = IoxsIoBaseToNet(g_device_data.pDeviceData[id].OutDatIocs);
+	pContent->pData = g_device_data.pDeviceData[id].OutData;
+	pContent->iops = IoxsIoBaseToNet(g_device_data.pDeviceData[id].OutDatIops);
+	/* substitute block header */
+	pContent->dsBlockTypeSubst = SWAP_16(DS_BLOCK_TYPE_SUBST_VALUE);
+	pContent->dsBlockLengthSubst = SWAP_16(pContent->dsBlockLengthSubst);
+	pContent->dsBlockVersionHiSubst = IEC_DS_VERSION_HIGH_1;
+	pContent->dsBlockVersionLoSubst = IEC_DS_VERSION_LOW_0;
+	/*substitute block body */
+	pContent->substMode = SWAP_16(0);
+	pContent->substIocs = pContent->iocs;
+	pContent->pSubstData = pContent->pData;
+}
+
+void DsFill_InputDataObject(IEC_DS_READ_INP_DATA_OBJ_ELEM* pContent, PNIO_UINT32* pBufLen, PNIO_UINT8* pBuf, PNIO_ERR_STAT* pPnioState)
+{
+	PNIO_UINT8* pAct = pBuf;
+	PNIO_UINT16 dsLen = SWAP_16(pContent->dsBlockLength) + sizeof(pContent->dsBlockType) + sizeof(pContent->dsBlockLength);
+	if (*pBufLen < (PNIO_UINT32)dsLen)
+	{
+		pPnioState->ErrCode   = 0xde;  /* IODReadRes */
+		pPnioState->ErrDecode = 0x80;  /* PNIORW                                */
+		pPnioState->ErrCode1  = 0xC0;  /* Resource / read constrain conflict */
+		pPnioState->ErrCode2  = 0;     /* here dont care                                                             */
+		pPnioState->AddValue1 = 0;     /* here dont care                                                             */
+		pPnioState->AddValue2 = 0;     /* here dont care                                                             */
+		return;
+	}
+	/* fill block header */
+	memcpy(pAct, &pContent->dsBlockType, sizeof(pContent->dsBlockType));
+	pAct += sizeof(pContent->dsBlockType);
+	memcpy(pAct, &pContent->dsBlockLength, sizeof(pContent->dsBlockLength));
+	pAct += sizeof(pContent->dsBlockLength);
+	*pAct++ = pContent->dsBlockVersionHi;
+	*pAct++ = pContent->dsBlockVersionLo;
+	/* fill block data */
+	*pAct++ = pContent->lengthIocs;
+	*pAct++ = pContent->iocs;
+	*pAct++ = pContent->lengthIops;
+	*pAct++ = pContent->iops;
+	memcpy(pAct, &pContent->lengthData, sizeof(pContent->lengthData));
+	pAct += sizeof(pContent->lengthData);
+	memcpy(pAct, pContent->pData, SWAP_16(pContent->lengthData));
+	memset(pPnioState, 0, sizeof(*pPnioState));
+	*pBufLen = (PNIO_UINT32)dsLen;
+}
+
+void DsFill_OutputDataObject(IEC_DS_READ_OUTP_DATA_OBJ_ELEM* pContent, PNIO_UINT32* pBufLen, PNIO_UINT8* pBuf, PNIO_ERR_STAT* pPnioState)
+{
+	PNIO_UINT8* pAct = pBuf;
+	PNIO_UINT16 dsLen =
+		SWAP_16(pContent->dsBlockLength)
+		+ sizeof(pContent->dsBlockType)
+		+ sizeof(pContent->dsBlockLength);
+	printf("dsBlockLength=%d, dsBlockLengthSubst=%d dsLen=%d bufLen=%d\n",
+		SWAP_16(pContent->dsBlockLength), SWAP_16(pContent->dsBlockLengthSubst),
+		dsLen, *pBufLen);
+	if (*pBufLen < (PNIO_UINT32)dsLen)
+	{
+		pPnioState->ErrCode   = 0xde;  /* IODReadRes */
+		pPnioState->ErrDecode = 0x80;  /* PNIORW                                */
+		pPnioState->ErrCode1  = 0xC0;  /* Resource / read constrain conflict */
+		pPnioState->ErrCode2  = 0;     /* here dont care                                                             */
+		pPnioState->AddValue1 = 0;     /* here dont care                                                             */
+		pPnioState->AddValue2 = 0;     /* here dont care                                                             */
+		return;
+	}
+	/* fill block header */
+	memcpy(pAct, &pContent->dsBlockType, sizeof(pContent->dsBlockType));
+	pAct += sizeof(pContent->dsBlockType);
+	memcpy(pAct, &pContent->dsBlockLength, sizeof(pContent->dsBlockLength));
+	pAct += sizeof(pContent->dsBlockLength);
+	*pAct++ = pContent->dsBlockVersionHi;
+	*pAct++ = pContent->dsBlockVersionLo;
+	/* fill block data */
+	memcpy(pAct, &pContent->substActiveFlag, sizeof(pContent->substActiveFlag));
+	pAct += sizeof(pContent->substActiveFlag);
+	*pAct++ = pContent->lengthIocs;
+	*pAct++ = pContent->lengthIops;
+	memcpy(pAct, &pContent->lengthData, sizeof(pContent->lengthData));
+	pAct += sizeof(pContent->lengthData);
+	*pAct++ = pContent->iocs;
+	memcpy(pAct, pContent->pData, SWAP_16(pContent->lengthData));
+	pAct += SWAP_16(pContent->lengthData);
+	*pAct++ = pContent->iops;
+	/* fill substitute block header */
+	memcpy(pAct, &pContent->dsBlockTypeSubst, sizeof(pContent->dsBlockTypeSubst));
+	pAct += sizeof(pContent->dsBlockTypeSubst);
+	memcpy(pAct, &pContent->dsBlockLengthSubst, sizeof(pContent->dsBlockLengthSubst));
+	pAct += sizeof(pContent->dsBlockLengthSubst);
+	*pAct++ = pContent->dsBlockVersionHiSubst;
+	*pAct++ = pContent->dsBlockVersionLoSubst;
+	/* fill substitute block body */
+	memcpy(pAct, &pContent->substMode, sizeof(pContent->substMode));
+	pAct += sizeof(pContent->substMode);
+	*pAct++ = pContent->iocs;
+	memcpy(pAct, pContent->pData, SWAP_16(pContent->lengthData));
+	pAct += SWAP_16(pContent->lengthData);
+	*pAct++ = pContent->substDataValid;
+	memset(pPnioState, 0, sizeof(*pPnioState));
+	*pBufLen = (PNIO_UINT32)dsLen;
+}

src/robot_control/src/cp1616/ds.h

@@ -0,0 +1,91 @@
+/*------------------------------------------------------------------------*/
+/* Copyright (C) SIEMENS CORP., 2018 All rights reserved.*/
+/* All rights reserved.                                                   */
+/* Redistribution and use in source and binary forms, with or without     */
+/* modification, are permitted provided that the following conditions     */
+/* are met:                                                               */
+/* 1. Redistributions of source code must retain the above copyright      */
+/* notice, this list of conditions and the following disclaimer.          */
+/* 2. Redistributions in binary form must reproduce the above copyright   */
+/* notice, this list of conditions and the following disclaimer in the    */
+/* documentation and/or other materials provided with the distribution.   */
+/* 3. Neither the name of the copyright holder nor the names of its       */
+/* contributors may be used to endorse or promote products derived from   */
+/* this software without specific prior written permission.               */
+/* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS    */
+/* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT      */
+/* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS      */
+/* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE         */
+/* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,   */
+/* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,   */
+/* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS  */
+/* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)                    */
+/* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,    */
+/* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING  */
+/* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE     */
+/* POSSIBILITY OF SUCH DAMAGE.                                            */
+/*------------------------------------------------------------------------*/
+/*------------------------------------------------------------------------*/
+/* Description: This module contains handling of data sets (without I&M). */
+/*                                                                        */
+/*------------------------------------------------------------------------*/
+/* Attention : Callbacks are running concurrent in other threads so all   */
+/*             printf statements should be synchronized. But this sample  */
+/*             application doesn't synchronize for simplicity  !          */
+/*------------------------------------------------------------------------*/
+
+#ifndef _DS_H
+#define _DS_H
+
+#include <pniousrd.h>
+
+#define DS_NUM_REC_INP_DATA_OBJ_ELEM  0x8028
+#define DS_NUM_REC_OUTP_DATA_OBJ_ELEM 0x8029
+
+/*  structure of IEC dataset header for dataset ReadInputDataObjectElement 0x8028 */
+typedef struct
+{
+	PNIO_UINT16 dsBlockType;
+	PNIO_UINT16 dsBlockLength;
+	PNIO_UINT8  dsBlockVersionHi;
+	PNIO_UINT8  dsBlockVersionLo;
+	PNIO_UINT8  lengthIocs;
+	PNIO_UINT8  iocs;
+	PNIO_UINT8  lengthIops;
+	PNIO_UINT8  iops;
+	PNIO_UINT16 lengthData;
+	PNIO_UINT8*  pData;
+} IEC_DS_READ_INP_DATA_OBJ_ELEM;
+
+/*  structure of IEC dataset header for dataset ReadOutputDataObjectElement 0x8029 */
+typedef struct
+{
+	PNIO_UINT16 dsBlockType;
+	PNIO_UINT16 dsBlockLength;
+	PNIO_UINT8  dsBlockVersionHi;
+	PNIO_UINT8  dsBlockVersionLo;
+	PNIO_UINT16 substActiveFlag;       /*  0x0000 */
+	PNIO_UINT8  lengthIocs;
+	PNIO_UINT8  lengthIops;
+	PNIO_UINT16 lengthData;
+	PNIO_UINT8  iocs;
+	PNIO_UINT8*  pData;
+	PNIO_UINT8  iops;
+
+	PNIO_UINT16 dsBlockTypeSubst;      /*  0x0014 */
+	PNIO_UINT16 dsBlockLengthSubst;    /*  7 */
+	PNIO_UINT8  dsBlockVersionHiSubst; /*  1 */
+	PNIO_UINT8  dsBlockVersionLoSubst; /*  0 */
+	PNIO_UINT16 substMode;             /*  always 0 here */
+	PNIO_UINT8 substIocs;              /*  same as iocs */
+	PNIO_UINT8*  pSubstData;
+	PNIO_UINT8 substDataValid;         /* always bad (0x40) */
+} IEC_DS_READ_OUTP_DATA_OBJ_ELEM;
+
+void DsInit_InputDataObject(IEC_DS_READ_INP_DATA_OBJ_ELEM* pContent, PNIO_DEV_ADDR* pAddr);
+void DsInit_OutputDataObject(IEC_DS_READ_OUTP_DATA_OBJ_ELEM* pContent, PNIO_DEV_ADDR* pAddr);
+void DsFill_InputDataObject(IEC_DS_READ_INP_DATA_OBJ_ELEM* pContent, PNIO_UINT32* pBufLen, PNIO_UINT8* pBuf, PNIO_ERR_STAT* pPnioState);
+void DsFill_OutputDataObject(IEC_DS_READ_OUTP_DATA_OBJ_ELEM* pContent, PNIO_UINT32* pBufLen, PNIO_UINT8* pBuf, PNIO_ERR_STAT* pPnioState);
+
+
+#endif /* _DS_H */

src/robot_control/src/cp1616/im.c

@@ -0,0 +1,936 @@
+/*------------------------------------------------------------------------*/
+/* Copyright (C) SIEMENS CORP., 2018 All rights reserved.*/
+/* All rights reserved.                                                   */
+/* Redistribution and use in source and binary forms, with or without     */
+/* modification, are permitted provided that the following conditions     */
+/* are met:                                                               */
+/* 1. Redistributions of source code must retain the above copyright      */
+/* notice, this list of conditions and the following disclaimer.          */
+/* 2. Redistributions in binary form must reproduce the above copyright   */
+/* notice, this list of conditions and the following disclaimer in the    */
+/* documentation and/or other materials provided with the distribution.   */
+/* 3. Neither the name of the copyright holder nor the names of its       */
+/* contributors may be used to endorse or promote products derived from   */
+/* this software without specific prior written permission.               */
+/* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS    */
+/* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT      */
+/* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS      */
+/* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE         */
+/* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,   */
+/* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,   */
+/* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS  */
+/* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)                    */
+/* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,    */
+/* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING  */
+/* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE     */
+/* POSSIBILITY OF SUCH DAMAGE.                                            */
+/*------------------------------------------------------------------------*/
+/*------------------------------------------------------------------------*/
+/* Attention : Callbacks are running concurrent in other threads so all   */
+/*             printf statements should be synchronized. But this sample  */
+/*             application doesn't synchronize for simplicity  !          */
+/*------------------------------------------------------------------------*/
+
+#include "im.h"
+#include "dev_certify_energy.h"
+
+#ifdef WIN32
+#include <windows.h>
+#include <stdio.h>
+#include <conio.h>
+#else
+#define _BSD_SOURCE /* for usleep */
+#include <stdio.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <poll.h>
+#include <termios.h>
+#include <string.h>
+#include <time.h>
+#define Sleep(x)  usleep(x*1000)
+#endif
+
+#include "servusrx.h"
+
+/*----------------------------------------------------------------------------------------------------*/
+/*    DEFINES                                                                                         */
+/*----------------------------------------------------------------------------------------------------*/
+
+#define STRNCMP_EQUAL 0
+#define LINE_LENGTH_MAX           255   /* Maximum line length of persistent im data file */
+#define NUMOF_PERSISTENT_IM_ITEMS 6     /* persistent: IM_Revision_Counter, IM_Tag_Function, IM_Tag_Location, IM_Date, IM_Descriptor, IM_Signature */
+
+#define LEN_IM_ORDERID 20
+#define LEN_IM_SERIAL_NUMBER 16
+#define LEN_IM_SOFTWARE_REVISION 4
+#define LEN_IM_TAG_FUNCTION 32
+#define LEN_IM_TAG_LOCATION 22
+#define LEN_IM_DATE 16
+#define LEN_IM_DESCRIPTOR 54
+#define LEN_IM_SIGNATURE 54
+
+/*----------------------------------------------------------------------------------------------------*/
+/*    TYPES                                                                                           */
+/*----------------------------------------------------------------------------------------------------*/
+
+typedef struct IM_PERMANENT_tag   /*size: 142*/
+{
+	PNIO_UINT16     VendorID;
+	PNIO_UINT16     OrderID;
+	PNIO_UINT8      pIM_Serial_Number[16];
+	PNIO_UINT16     IM_Hardware_Revision;
+	PNIO_UINT8      IM_Software_Revision[4];
+	PNIO_UINT16     IM_Profile_ID;
+	PNIO_UINT16     IM_Profile_Specific_Type;
+	PNIO_UINT16     IM_Version;
+	PNIO_UINT16     IM_Supported;
+	PNIO_UINT8      IM_Tag_Function[32];
+	PNIO_UINT8      IM_Tag_Location[22];
+	PNIO_UINT8      IM_Date[16];
+	PNIO_UINT8      IM_Descriptor[54];
+	PNIO_UINT8      IM_Signature[54];
+} IM_PERMANENT;
+
+typedef struct tag_IM_DATA
+{
+	PNIO_IM0_TYPE   im0;
+	PNIO_IM1_TYPE   im1;
+	PNIO_IM2_TYPE   im2;
+	PNIO_IM3_TYPE   im3;
+	PNIO_IM4_TYPE   im4;
+} IM_DATA;
+
+typedef enum
+{
+	IM_Revision_Counter =       0,
+	IM_Tag_Function =           1,
+	IM_Tag_Location =           2,
+	IM_Date =                   3,
+	IM_Descriptor =             4,
+	IM_Signature =              5
+} IM_PERSISTENT_DATA_e;
+
+typedef enum
+{
+	IM_EQUAL = 0,
+	IM_READ_DIFFER = 1,
+	IM_WRITE_DIFFER = 2,
+	IM_READ_WRITE_DIFFER = 3
+} IM_CMP_E;
+
+typedef struct tag_IM_PERSISTENT_DATA   /* persistent IM data */
+{
+	PNIO_UINT16     IM_Revision_Counter;
+	PNIO_UINT8      IM_Tag_Function[LEN_IM_TAG_FUNCTION];
+	PNIO_UINT8      IM_Tag_Location[LEN_IM_TAG_LOCATION];
+	PNIO_UINT8      IM_Date[LEN_IM_DATE];
+	PNIO_UINT8      IM_Descriptor[LEN_IM_DESCRIPTOR];
+	PNIO_UINT8      IM_Signature[LEN_IM_SIGNATURE];
+} IM_PERSISTENT_DATA;
+
+/*----------------------------------------------------------------------------------------------------*/
+/*    I&M DATA                                                                                        */
+/*----------------------------------------------------------------------------------------------------*/
+
+static IM_DATA* im_work = 0;             /* current IM data */
+static const char* name_IM_PERSISTENT_DATA[] =      /* names of persistent IM data items */
+{
+	"IM_Revision_Counter",
+	"IM_Tag_Function",
+	"IM_Tag_Location",
+	"IM_Date",
+	"IM_Descriptor",
+	"IM_Signature"
+};
+
+static const IM_DATA im_default =   /* default IM data */
+{
+	{   /* I&M0 */
+		{
+			0x0020,                     /* BlockHeader.BlockType */
+			0x0038,                     /* BlockHeader.BlockLength */
+			0x01,                       /* BlockHeader.BlockVersionHigh */
+			0x00						/* BlockHeader.BlockVersionLow */
+		},                      
+		0x00,							/* VendorIDHigh - will be set to value from readConfig! */
+		0x2A,							/* VendorIDLow  - will be set to value from readConfig! */
+		{ ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', }, /* OrderID[20] -> will be set to selected device annotation! */
+		{ '0', '0', '0', '-', '0', '0', '0', '-', '0', '0', '0', '-', '0', '0', '0', '1' }, /* IM_Serial_Number[16] -> will be set to actual serial number! */
+		9,					            /* IM_Hardware_Revision - will be set to selected device annotation! */
+		{ 'V', 2, 7, 0 },               /* IM_Software_Revision[4] - will be set to selected device annotation! */
+		0x0000,							/* IM_Revision_Counter */
+		0x0000,							/* IM_Profile_ID -> Generic Device */
+		0x0003,							/* IM_Profile_Specific_Type -> Communication Module -> www.profibus.com/IM/Device_ID_Table_6102.xml */
+		0x01,							/* IM_Version_Major */
+		0x01,							/* IM_Version_Minor */
+		0x003e							/* IM_Supported -> IM0 bis IM4 */
+	},
+	{   /* I&M1 */
+		{
+			0x0021,                     /* BlockHeader.BlockType */
+			0x0038,                     /* BlockHeader.BlockLength */
+			0x01,                       /* BlockHeader.BlockVersionHigh */
+			0x00						/* BlockHeader.BlockVersionLow */
+		},
+		{ ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ' }, /* IM_Tag_Function[32] */
+		{ ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ' } /* IM_Tag_Location[22] */
+	},
+	{  /* I&M2 */
+		{
+			0x0022,                     /* BlockHeader.BlockType */
+			0x0012,                     /* BlockHeader.BlockLength */
+			0x01,                       /* BlockHeader.BlockVersionHigh */
+			0x00						/* BlockHeader.BlockVersionLow */
+		},
+		{ ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ' } /* IM_Date[16] */
+	},
+	{  /* I&M3 */
+		{
+			0x0023,                     /* BlockHeader.BlockType */
+			0x0038,                     /* BlockHeader.BlockLength */
+			0x01,                       /* BlockHeader.BlockVersionHigh */
+			0x00						/* BlockHeader.BlockVersionLow */
+		},
+		{ ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ' } /* IM_Descriptor[54] */
+	},
+	{   /* I&M4 */
+		{
+			0x0024,                     /* BlockHeader.BlockType */
+			0x0038,                     /* BlockHeader.BlockLength */
+			0x01,                       /* BlockHeader.BlockVersionHigh */
+			0x00						/* BlockHeader.BlockVersionLow */
+		},
+		{ '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0' } /* IM_Signature[54] */
+	}
+};
+
+
+
+
+/*----------------------------------------------------------------------------------------------------*/
+/*                  I&M internal functions                                                            */
+/*----------------------------------------------------------------------------------------------------*/
+
+void IM_incRevCounter(PNIO_UINT16* pRevCounter)
+{
+	if (*pRevCounter == 0)
+	{
+		*pRevCounter = 1;
+	}
+}
+
+IM_CMP_E IM_CmpIM0(const PNIO_IM0_TYPE* pLeft, const PNIO_IM0_TYPE* pRight)
+{
+	if (
+		pLeft->BlockHeader.BlockLength == pRight->BlockHeader.BlockLength ||
+		pLeft->BlockHeader.BlockType == pRight->BlockHeader.BlockType ||
+		pLeft->BlockHeader.BlockVersionHigh == pRight->BlockHeader.BlockVersionHigh ||
+		pLeft->BlockHeader.BlockVersionLow == pRight->BlockHeader.BlockVersionLow ||
+		pLeft->IM_Hardware_Revision == pRight->IM_Hardware_Revision ||
+		pLeft->IM_Profile_ID == pRight->IM_Profile_ID ||
+		pLeft->IM_Profile_Specific_Type == pRight->IM_Profile_Specific_Type ||
+		pLeft->IM_Revision_Counter == pRight->IM_Revision_Counter ||
+		pLeft->IM_Supported == pRight->IM_Supported ||
+		pLeft->IM_Version_Major == pRight->IM_Version_Major ||
+		pLeft->IM_Version_Minor == pRight->IM_Version_Minor ||
+		STRNCMP_EQUAL == strncmp((char*)pLeft->OrderID, (char*)pRight->OrderID, LEN_IM_ORDERID) ||
+		pLeft->VendorIDHigh == pRight->VendorIDHigh ||
+		pLeft->VendorIDLow == pRight->VendorIDLow ||
+		STRNCMP_EQUAL == strncmp((char*)pLeft->IM_Serial_Number, (char*)pRight->IM_Serial_Number, LEN_IM_SERIAL_NUMBER) ||
+		STRNCMP_EQUAL == strncmp((char*)pLeft->IM_Software_Revision, (char*)pRight->IM_Software_Revision, LEN_IM_SOFTWARE_REVISION)
+		)
+	{
+		return IM_EQUAL;
+	}
+	return IM_READ_DIFFER;
+}
+
+IM_CMP_E IM_CmpIM1(const PNIO_IM1_TYPE* pLeft, const PNIO_IM1_TYPE* pRight)
+{
+	PNIO_BOOL readDiffer = PNIO_TRUE;
+	PNIO_BOOL writeDiffer = PNIO_TRUE;
+	if (pLeft->BlockHeader.BlockLength == pRight->BlockHeader.BlockLength ||
+		pLeft->BlockHeader.BlockType == pRight->BlockHeader.BlockType ||
+		pLeft->BlockHeader.BlockVersionHigh == pRight->BlockHeader.BlockVersionHigh ||
+		pLeft->BlockHeader.BlockVersionLow == pRight->BlockHeader.BlockVersionLow)
+	{
+		readDiffer = PNIO_FALSE;
+	}
+	if (STRNCMP_EQUAL == strncmp((char*)pLeft->IM_Tag_Function, (char*)pRight->IM_Tag_Function, LEN_IM_TAG_FUNCTION) &&
+		STRNCMP_EQUAL == strncmp((char*)pLeft->IM_Tag_Location, (char*)pRight->IM_Tag_Location, LEN_IM_TAG_LOCATION))
+	{
+		writeDiffer = PNIO_FALSE;
+	}
+	if ((readDiffer == PNIO_TRUE) && (writeDiffer == PNIO_TRUE))
+	{
+		return IM_READ_WRITE_DIFFER;
+	}
+	if ((readDiffer == PNIO_TRUE) && (writeDiffer == PNIO_FALSE))
+	{
+		return IM_READ_DIFFER;
+	}
+	if ((readDiffer == PNIO_FALSE) && (writeDiffer == PNIO_TRUE))
+	{
+		return IM_WRITE_DIFFER;
+	}
+	return IM_EQUAL;
+}
+
+IM_CMP_E IM_CmpIM2(const PNIO_IM2_TYPE* pLeft, const PNIO_IM2_TYPE* pRight)
+{
+	PNIO_BOOL readDiffer = PNIO_TRUE;
+	PNIO_BOOL writeDiffer = PNIO_TRUE;
+	if (pLeft->BlockHeader.BlockLength == pRight->BlockHeader.BlockLength ||
+		pLeft->BlockHeader.BlockType == pRight->BlockHeader.BlockType ||
+		pLeft->BlockHeader.BlockVersionHigh == pRight->BlockHeader.BlockVersionHigh ||
+		pLeft->BlockHeader.BlockVersionLow == pRight->BlockHeader.BlockVersionLow)
+	{
+		readDiffer = PNIO_FALSE;
+	}
+	if (STRNCMP_EQUAL == strncmp((char*)pLeft->IM_Date, (char*)pRight->IM_Date, LEN_IM_DATE))
+	{
+		writeDiffer = PNIO_FALSE;
+	}
+	if ((readDiffer == PNIO_TRUE) && (writeDiffer == PNIO_TRUE))
+	{
+		return IM_READ_WRITE_DIFFER;
+	}
+	if ((readDiffer == PNIO_TRUE) && (writeDiffer == PNIO_FALSE))
+	{
+		return IM_READ_DIFFER;
+	}
+	if ((readDiffer == PNIO_FALSE) && (writeDiffer == PNIO_TRUE))
+	{
+		return IM_WRITE_DIFFER;
+	}
+	return IM_EQUAL;
+}
+
+IM_CMP_E IM_CmpIM3(const PNIO_IM3_TYPE* pLeft, const PNIO_IM3_TYPE* pRight)
+{
+	PNIO_BOOL readDiffer = PNIO_TRUE;
+	PNIO_BOOL writeDiffer = PNIO_TRUE;
+	if (pLeft->BlockHeader.BlockLength == pRight->BlockHeader.BlockLength ||
+		pLeft->BlockHeader.BlockType == pRight->BlockHeader.BlockType ||
+		pLeft->BlockHeader.BlockVersionHigh == pRight->BlockHeader.BlockVersionHigh ||
+		pLeft->BlockHeader.BlockVersionLow == pRight->BlockHeader.BlockVersionLow)
+	{
+		readDiffer = PNIO_FALSE;
+	}
+	if (STRNCMP_EQUAL == strncmp((char*)pLeft->IM_Descriptor, (char*)pRight->IM_Descriptor, LEN_IM_DESCRIPTOR))
+	{
+		writeDiffer = PNIO_FALSE;
+	}
+	if ((readDiffer == PNIO_TRUE) && (writeDiffer == PNIO_TRUE))
+	{
+		return IM_READ_WRITE_DIFFER;
+	}
+	if ((readDiffer == PNIO_TRUE) && (writeDiffer == PNIO_FALSE))
+	{
+		return IM_READ_DIFFER;
+	}
+	if ((readDiffer == PNIO_FALSE) && (writeDiffer == PNIO_TRUE))
+	{
+		return IM_WRITE_DIFFER;
+	}
+	return IM_EQUAL;
+}
+
+IM_CMP_E IM_CmpIM4(const PNIO_IM4_TYPE* pLeft, const PNIO_IM4_TYPE* pRight)
+{
+	PNIO_BOOL readDiffer = PNIO_TRUE;
+	PNIO_BOOL writeDiffer = PNIO_TRUE;
+	if (pLeft->BlockHeader.BlockLength == pRight->BlockHeader.BlockLength ||
+		pLeft->BlockHeader.BlockType == pRight->BlockHeader.BlockType ||
+		pLeft->BlockHeader.BlockVersionHigh == pRight->BlockHeader.BlockVersionHigh ||
+		pLeft->BlockHeader.BlockVersionLow == pRight->BlockHeader.BlockVersionLow)
+	{
+		readDiffer = PNIO_FALSE;
+	}
+	if (STRNCMP_EQUAL == strncmp((char*)pLeft->IM_Signature, (char*)pRight->IM_Signature, LEN_IM_SIGNATURE))
+	{
+		writeDiffer = PNIO_FALSE;
+	}
+	if ((readDiffer == PNIO_TRUE) && (writeDiffer == PNIO_TRUE))
+	{
+		return IM_READ_WRITE_DIFFER;
+	}
+	if ((readDiffer == PNIO_TRUE) && (writeDiffer == PNIO_FALSE))
+	{
+		return IM_READ_DIFFER;
+	}
+	if ((readDiffer == PNIO_FALSE) && (writeDiffer == PNIO_TRUE))
+	{
+		return IM_WRITE_DIFFER;
+	}
+	return IM_EQUAL;
+}
+
+PNIO_UINT8* IM_RemTrailBlanks(const PNIO_UINT8* pBuf, int bufLen)
+{
+	static PNIO_UINT8 pNoBlanks[LINE_LENGTH_MAX];
+	int idx = bufLen - 1;
+	int copyLen = 0;
+	if (bufLen >= LINE_LENGTH_MAX)
+	{
+		idx = LINE_LENGTH_MAX - 2;
+	}
+	for (; idx >= 0; idx--)
+	{
+		if (pBuf[idx] != ' ')
+		{
+			copyLen = idx + 1;
+			break;
+		}
+		if (idx == 0)         /* solely blanks found */
+		{
+			copyLen = 0;
+			break;
+		}
+	}
+	strncpy((char*)pNoBlanks, (char*)pBuf, copyLen);
+	pNoBlanks[copyLen] = '\0';
+	return pNoBlanks;
+}
+
+/* pSrc has to be null-terminated */
+PNIO_BOOL IM_CopyQuotedString(PNIO_UINT8* pDst, PNIO_UINT8* pSrc, size_t maxSize)
+{
+	PNIO_UINT8* pTmp;
+	size_t copyLen;
+	pSrc = (PNIO_UINT8*) strchr((char*)pSrc, '"');
+	if (!pSrc)
+	{
+		return PNIO_FALSE;
+	}
+	else
+	{
+		pSrc++;
+	}
+	pTmp = (PNIO_UINT8*)strrchr((char*)pSrc, '"');
+	if (!pTmp)
+	{
+		return PNIO_FALSE;
+	}
+	copyLen = pTmp - pSrc;
+	if (copyLen > maxSize)
+	{
+		copyLen = maxSize;
+	}
+	strncpy((char*)pDst, (char*)pSrc, copyLen);
+	return PNIO_TRUE;
+}
+
+void IM_CopySwapIm1FromBuf(
+	PNIO_IM1_TYPE*      pIm, /* [out] */
+	PNIO_UINT8*  const pBuf) /* [in]  */
+{
+	*pIm =                          *(PNIO_IM1_TYPE*)pBuf;
+	pIm->BlockHeader.BlockType =                SWAP_16(((PNIO_IM1_TYPE*)pBuf)->BlockHeader.BlockType);
+	pIm->BlockHeader.BlockLength =              SWAP_16(((PNIO_IM1_TYPE*)pBuf)->BlockHeader.BlockLength);
+}
+
+void IM_CopySwapIm2FromBuf(
+	PNIO_IM2_TYPE*      pIm,        /* [out] */
+	PNIO_UINT8*  const pBuf) /* [in]  */
+{
+	*pIm =                          *(PNIO_IM2_TYPE*)pBuf;
+	pIm->BlockHeader.BlockType =                SWAP_16(((PNIO_IM2_TYPE*)pBuf)->BlockHeader.BlockType);
+	pIm->BlockHeader.BlockLength =              SWAP_16(((PNIO_IM2_TYPE*)pBuf)->BlockHeader.BlockLength);
+}
+
+void IM_CopySwapIm3FromBuf(
+	PNIO_IM3_TYPE*      pIm, /* [out] */
+	PNIO_UINT8*  const pBuf) /* [in]  */
+{
+	*pIm =                          *(PNIO_IM3_TYPE*)pBuf;
+	pIm->BlockHeader.BlockType =                SWAP_16(((PNIO_IM3_TYPE*)pBuf)->BlockHeader.BlockType);
+	pIm->BlockHeader.BlockLength =              SWAP_16(((PNIO_IM3_TYPE*)pBuf)->BlockHeader.BlockLength);
+}
+
+void IM_CopySwapIm4FromBuf(
+	PNIO_IM4_TYPE*      pIm, /* [out] */
+	PNIO_UINT8*  const pBuf) /* [in]  */
+{
+	*pIm =                          *(PNIO_IM4_TYPE*)pBuf;
+	pIm->BlockHeader.BlockType =                SWAP_16(((PNIO_IM4_TYPE*)pBuf)->BlockHeader.BlockType);
+	pIm->BlockHeader.BlockLength =              SWAP_16(((PNIO_IM4_TYPE*)pBuf)->BlockHeader.BlockLength);
+}
+
+PNIO_BOOL IM_CopySwapBufFromIm(
+	PNIO_UINT8*    pBuf, /* [out]  */
+	IM_DATA const* pIm,  /* [in] */
+	IM0_idx_e     idx)   /* [in]  */
+{
+	switch (idx)
+	{
+	case IM0:
+		*((PNIO_IM0_TYPE*)pBuf)           =                pIm->im0;
+		((PNIO_IM0_TYPE*)pBuf)->BlockHeader.BlockType =    SWAP_16(pIm->im0.BlockHeader.BlockType);
+		((PNIO_IM0_TYPE*)pBuf)->BlockHeader.BlockLength =  SWAP_16(pIm->im0.BlockHeader.BlockLength);
+		((PNIO_IM0_TYPE*)pBuf)->IM_Hardware_Revision =     SWAP_16(pIm->im0.IM_Hardware_Revision);
+		/* ((PNIO_IM0_TYPE *)pBuf)->IM_Software_Revision =     SWAP_32(pIm->im0.IM_Software_Revision);*/
+		((PNIO_IM0_TYPE*)pBuf)->IM_Revision_Counter =      SWAP_16(pIm->im0.IM_Revision_Counter);
+		((PNIO_IM0_TYPE*)pBuf)->IM_Profile_Specific_Type = SWAP_16(pIm->im0.IM_Profile_Specific_Type);
+		((PNIO_IM0_TYPE*)pBuf)->IM_Profile_ID =            SWAP_16(pIm->im0.IM_Profile_ID);
+		((PNIO_IM0_TYPE*)pBuf)->IM_Supported =             SWAP_16(pIm->im0.IM_Supported);
+		break;
+	case IM1:
+		*((PNIO_IM1_TYPE*)pBuf)           =                pIm->im1;
+		((PNIO_IM1_TYPE*)pBuf)->BlockHeader.BlockType =     SWAP_16(pIm->im1.BlockHeader.BlockType);
+		((PNIO_IM1_TYPE*)pBuf)->BlockHeader.BlockLength =   SWAP_16(pIm->im1.BlockHeader.BlockLength);
+		break;
+	case IM2:
+		*((PNIO_IM2_TYPE*)pBuf)           =                pIm->im2;
+		((PNIO_IM2_TYPE*)pBuf)->BlockHeader.BlockType =     SWAP_16(pIm->im2.BlockHeader.BlockType);
+		((PNIO_IM2_TYPE*)pBuf)->BlockHeader.BlockLength =   SWAP_16(pIm->im2.BlockHeader.BlockLength);
+		break;
+	case IM3:
+		*((PNIO_IM3_TYPE*)pBuf)           =                pIm->im3;
+		((PNIO_IM3_TYPE*)pBuf)->BlockHeader.BlockType =     SWAP_16(pIm->im3.BlockHeader.BlockType);
+		((PNIO_IM3_TYPE*)pBuf)->BlockHeader.BlockLength =   SWAP_16(pIm->im3.BlockHeader.BlockLength);
+		break;
+	case IM4:
+		*((PNIO_IM4_TYPE*)pBuf)           =                pIm->im4;
+		((PNIO_IM4_TYPE*)pBuf)->BlockHeader.BlockType =     SWAP_16(pIm->im4.BlockHeader.BlockType);
+		((PNIO_IM4_TYPE*)pBuf)->BlockHeader.BlockLength =   SWAP_16(pIm->im4.BlockHeader.BlockLength);
+		break;
+	default:
+		break;
+	}
+	return PNIO_TRUE;
+}
+/* Diese Funktion gibt die Groesse des Datensatzes zurueck */
+PNIO_UINT32 IM_SizeofImDatarec(PNIO_UINT32  RecordIndex)
+{
+	switch (RecordIndex)
+	{
+	case IM0:
+		return (sizeof(PNIO_IM0_TYPE));
+	case IM1:
+		return (sizeof(PNIO_IM1_TYPE));
+	case IM2:
+		return (sizeof(PNIO_IM2_TYPE));
+	case IM3:
+		return (sizeof(PNIO_IM3_TYPE));
+	case IM4:
+		return (sizeof(PNIO_IM4_TYPE));
+	default:
+		return 0;
+	}
+}
+
+/* check if the given RacordIndex is supported */
+PNIO_BOOL IM_IndexSupported(PNIO_UINT32 RecordIndex, PNIO_UINT16 IM_Supported)
+{
+	/* && (IM_Supported&0x01) */
+	if (((RecordIndex == IM0)) ||
+		((RecordIndex == IM1) && (IM_Supported & 0x02)) ||
+		((RecordIndex == IM2) && (IM_Supported & 0x04)) ||
+		((RecordIndex == IM3) && (IM_Supported & 0x08)) ||
+		((RecordIndex == IM4) && (IM_Supported & 0x10)))
+	{
+		return PNIO_TRUE;
+	}
+	return PNIO_FALSE;
+}
+
+/* set error code */
+void setPnioRwError(PNIO_ERR_STAT* pPnioState, PNIO_UINT8 ErrCode1)
+{
+	/**** if an error occured, you must specify it according IEC 61158-6 */
+	pPnioState->ErrCode   = 0xdf;  /* IODWriteRes with ErrorDecode = PNIORW */
+	pPnioState->ErrDecode = 0x80;  /* PNIORW                                */
+	pPnioState->ErrCode1  = ErrCode1;
+	pPnioState->ErrCode2  = 0;     /* here dont care                                                             */
+	pPnioState->AddValue1 = 0;     /* here dont care                                                             */
+	pPnioState->AddValue2 = 0;     /* here dont care                                                             */
+}
+
+/*----------------------------------------------------------------------------------------------------*/
+/*                  I&M external functions                                                            */
+/*----------------------------------------------------------------------------------------------------*/
+
+/*  Read I&M data from pBuf */
+void IM_recWrite(PNIO_UINT32 RecordIndex, PNIO_DEV_ADDR Addr, PNIO_UINT32* pBufLen, PNIO_UINT8* pBuf, PNIO_ERR_STAT* pPnioState)
+{
+	IM_CMP_E    imCmp = IM_EQUAL;
+	IM_DATA*     pIm = NULL;
+	int id = 0;
+
+	*pBufLen = 0;
+	id = addrToId(Addr);
+	if (id < 0)
+	{
+		setPnioRwError(pPnioState, 0xb2); /* access: invalid slot/subslot */
+		return;
+	}
+	pIm = &im_work[id];
+	if (!IM_IndexSupported(RecordIndex, im_work[id].im0.IM_Supported))
+	{
+		setPnioRwError(pPnioState, 0xa9); /* application: feature not supported */
+		return;
+	}
+	switch (RecordIndex)
+	{
+	case IM0:
+		setPnioRwError(pPnioState, 0xb6); /* access: access denied */
+		return;
+	case IM1:
+		{
+			PNIO_IM1_TYPE im;
+			if (pBuf == 0)
+			{
+				im = im_default.im1;
+			}
+			else
+			{
+				IM_CopySwapIm1FromBuf(&im, pBuf);
+			}
+			imCmp = IM_CmpIM1(&im, &pIm->im1);
+			if ((imCmp == IM_READ_DIFFER) || (imCmp == IM_READ_WRITE_DIFFER))
+			{
+				setPnioRwError(pPnioState, 0xb4); /* Access: invalid area */
+			}
+			else
+				if (imCmp == IM_WRITE_DIFFER)
+				{
+					pIm->im1 = im;
+				}
+		}
+		break;
+	case IM2:
+		{
+			PNIO_IM2_TYPE im;
+			if (pBuf == 0)
+			{
+				im = im_default.im2;
+			}
+			else
+			{
+				IM_CopySwapIm2FromBuf(&im, pBuf);
+			}
+			imCmp = IM_CmpIM2(&im, &pIm->im2);
+			if ((imCmp == IM_READ_DIFFER) || (imCmp == IM_READ_WRITE_DIFFER))
+			{
+				setPnioRwError(pPnioState, 0xb4); /* Access: invalid area */
+			}
+			else
+				if (imCmp == IM_WRITE_DIFFER)
+				{
+					pIm->im2 = im;
+				}
+		}
+		break;
+	case IM3:
+		{
+			PNIO_IM3_TYPE im;
+			if (pBuf == 0)
+			{
+				im = im_default.im3;
+			}
+			else
+			{
+				IM_CopySwapIm3FromBuf(&im, pBuf);
+			}
+			imCmp = IM_CmpIM3(&im, &pIm->im3);
+			if ((imCmp == IM_READ_DIFFER) || (imCmp == IM_READ_WRITE_DIFFER))
+			{
+				setPnioRwError(pPnioState, 0xb4); /* Access: invalid area */
+			}
+			else
+				if (imCmp == IM_WRITE_DIFFER)
+				{
+					pIm->im3 = im;
+				}
+		}
+		break;
+	case IM4:
+		setPnioRwError(pPnioState, 0xb6); /* access: access denied */
+		break;
+	default:
+		break;
+	}
+
+	*pBufLen = IM_SizeofImDatarec(RecordIndex);
+
+	/* store data in any case (e.g. switched off) */
+	if( imCmp == IM_WRITE_DIFFER)
+	{
+        /* do not increment. always 0 */
+        /* IM_incRevCounter(&pIm->im0.IM_Revision_Counter); */
+	    IM_DataToStore("im.conf");
+	}
+
+	return;
+}
+
+/*  Write I&M data into pBuf */
+void IM_RecRead(PNIO_UINT32 RecordIndex, PNIO_DEV_ADDR Addr, PNIO_UINT32* pBufLen, PNIO_UINT8* pBuf, PNIO_ERR_STAT* pPnioState)
+{
+	int id = addrToId(Addr);
+	if (id < 0)
+	{
+		setPnioRwError(pPnioState, 0xb2); /* access: invalid slot/subslot */
+		return;
+	}
+
+	if (!IM_IndexSupported(RecordIndex, im_work[id].im0.IM_Supported))
+	{
+		setPnioRwError(pPnioState, 0xa9); /* application: feature not supported */
+		return;
+	}
+	*pBufLen = IM_SizeofImDatarec(RecordIndex);
+	IM_CopySwapBufFromIm(pBuf, &im_work[id], (IM0_idx_e)RecordIndex);
+	return;
+}
+
+int copy_string(char* dstBuf, const char* srcBuf, size_t len)
+{
+	int n;
+	for (n=0; n<len; n++)
+	{
+		if ((dstBuf[n] < 0x20) || (srcBuf[n] > 0x7E))
+		{
+			/* Control character */
+			dstBuf[n] = ' ';
+		}
+		else
+		{
+			/* Visible character */
+			dstBuf[n] = srcBuf[n];
+		}
+	}
+
+    return 1;
+}
+
+void set_serial_string( char* serial_str)
+{    
+	int n = 0;
+    size_t serial_length = 0;
+
+    if( !g_annot_serial)
+    {
+        g_annot_serial = (PNIO_UINT8*)malloc(MAX_SERIAL_LENGTH+1);
+    }
+
+    memset( g_annot_serial, ' ' , MAX_SERIAL_LENGTH+1);
+    g_annot_serial[MAX_SERIAL_LENGTH+1] = 0;
+
+    serial_length = strlen(serial_str);
+    if( serial_length > MAX_SERIAL_LENGTH-1) serial_length = MAX_SERIAL_LENGTH-1;
+    copy_string((char*)g_annot_serial,serial_str,serial_length);
+}
+
+void IM_initImRecords(void)
+{
+	int i;
+	SERV_CP_FW_INFO_TYPE servInfo;
+    SERV_CP_ANNOTATION_TYPE annoInfo;
+    
+	/* get actual serial number only on first call to this function, since the serial number can't change during runtime */
+	if (g_annot_serial == 0)
+    { 
+        g_annot_serial = (PNIO_UINT8*)malloc(sizeof(annoInfo.serial_number));
+        memset(g_annot_serial,0,sizeof(annoInfo.serial_number));
+        if( SERV_CP_get_annotation_data(1, &annoInfo) == PNIO_OK &&
+            SERV_CP_get_fw_info(1,&servInfo) == PNIO_OK)
+	    {
+            set_serial_string( annoInfo.mode == PNIO_CP_ANNO_COMPLETE ?
+                                    (char*)&annoInfo.serial_number[0] :
+                                    (char*)&servInfo.cp_ser_nr[0]);
+#if 0
+            size_t length = 0;
+        
+            if( annoInfo.mode != PNIO_CP_ANNO_NOT_USED)
+            {
+                length = strlen(annoInfo.serial_number);
+                if( length > sizeof(servInfo.cp_ser_nr)) /* maximum of 16 */
+                {
+                        length = sizeof(servInfo.cp_ser_nr); 
+                }
+
+                memcpy(&g_annot_serial[0], &annoInfo.serial_number[0], length);
+            }
+            else if ((SERV_CP_get_fw_info(1, &servInfo) == PNIO_OK))
+            {
+                length = strlen(servInfo.cp_ser_nr);
+                if( length > sizeof(servInfo.cp_ser_nr)) /* maximum of 16 */
+                {
+                    length = sizeof(servInfo.cp_ser_nr);
+                }
+            
+                memcpy(&g_annot_serial[0], &servInfo.cp_ser_nr[0], length);
+		    }
+            else
+            {
+                /* failed */
+            }
+#endif
+	    }
+        else
+        {
+            /* failed to get fw info */
+        }
+    }
+
+	free(im_work);
+	im_work = (IM_DATA*)malloc(sizeof(IM_DATA) * g_device_data.nrOfSubmodules);
+	for (i = 0; i < g_device_data.nrOfSubmodules; i++)
+	{
+		im_work[i] = im_default;
+		
+		im_work[i].im0.VendorIDHigh = (g_device_data.VendorId >> 8) & 0xff;
+		im_work[i].im0.VendorIDLow = g_device_data.VendorId & 0xff;
+		memcpy(&im_work[i].im0.OrderID, g_annot_selected.OrderId, MAX_ORDER_ID_LENGTH);
+		im_work[i].im0.IM_Hardware_Revision = g_annot_selected.HwRevision;
+		im_work[i].im0.IM_Software_Revision[0] = (PNIO_UINT8)g_annot_selected.SwRevisionPrefix;
+		im_work[i].im0.IM_Software_Revision[1] = (PNIO_UINT8)g_annot_selected.SwRevision1;
+		im_work[i].im0.IM_Software_Revision[2] = (PNIO_UINT8)g_annot_selected.SwRevision2;
+		im_work[i].im0.IM_Software_Revision[3] = (PNIO_UINT8)g_annot_selected.SwRevision3;
+		memcpy(&im_work[i].im0.IM_Serial_Number, g_annot_serial, sizeof(im_work[i].im0.IM_Serial_Number));
+
+		if ((g_device_data.pDeviceData[i].Addr.u.Geo.Slot == 1) &&
+			((g_device_data.pDeviceData[i].Addr.u.Geo.Subslot == 0x8000)
+			|| (g_device_data.pDeviceData[i].Addr.u.Geo.Subslot == 0x8001)
+			|| (g_device_data.pDeviceData[i].Addr.u.Geo.Subslot == 0x8002)
+			|| (g_device_data.pDeviceData[i].Addr.u.Geo.Subslot == 0x8003)
+			|| (g_device_data.pDeviceData[i].Addr.u.Geo.Subslot == 0x8004)))
+		{
+			im_work[i].im0.IM_Profile_Specific_Type = 0x04; /* communication module -> www.profibus.com/IM/Device_ID_Table_6102.xml */
+		}
+	}
+}
+
+void IM_DataFromStore(const char* pName)
+{
+	FILE* pFile;
+	PNIO_UINT8 line[LINE_LENGTH_MAX];
+	IM_PERSISTENT_DATA_e idx;
+	int sModIdx;
+	pFile = fopen(pName, "r");
+	if (pFile == NULL)
+	{
+		return;
+	}
+
+	for (sModIdx = 0; sModIdx < g_device_data.nrOfSubmodules; sModIdx++)
+	{
+		IM_DATA* pIm = &im_work[sModIdx];
+		/* override default IM values by stored values */
+		while (fgets((char*)line, LINE_LENGTH_MAX, pFile) != NULL)
+		{
+			char* pPos = NULL;
+			int sectionIdx;
+			/* ignore comments -> read next line */
+			if ((line[0] == ';') || (line[0] == '#'))
+			{
+				continue;
+			}
+			/* continue until submodule section is found */
+			sscanf((char*)line, "[SUBMOD%i]", &sectionIdx);
+			if (sectionIdx != sModIdx)
+			{
+				continue;
+			}
+			/* find IM item in current line */
+			for (idx = (IM_PERSISTENT_DATA_e)0; idx < NUMOF_PERSISTENT_IM_ITEMS; idx = (IM_PERSISTENT_DATA_e)((int)idx + 1))
+			{
+				size_t size = strlen(name_IM_PERSISTENT_DATA[idx]);
+				if (STRNCMP_EQUAL ==  strncmp((char*)line, name_IM_PERSISTENT_DATA[idx], size))
+				{
+					pPos = (char*)line + size;
+					/* ignore blanks etc */
+					while ((*pPos == ' ') || (*pPos == '='))
+					{
+						pPos++;
+					}
+					break;
+				}
+			}
+			if (pPos == NULL)     /* no IM item found */
+			{
+				continue;
+			}
+			/* read IM value */
+			switch (idx)
+			{
+			case IM_Revision_Counter:
+				{
+					int val;
+					sscanf(pPos, "%i", &val);
+					if ((val > 0) && (val <= 0xffff))
+					{
+						pIm->im0.IM_Revision_Counter = (PNIO_UINT16) val;
+					}
+					break;
+				}
+			case IM_Tag_Function:
+				IM_CopyQuotedString(pIm->im1.IM_Tag_Function, line, sizeof(pIm->im1.IM_Tag_Function));
+				break;
+			case IM_Tag_Location:
+				IM_CopyQuotedString(pIm->im1.IM_Tag_Location, line, sizeof(pIm->im1.IM_Tag_Location));
+				break;
+			case IM_Date:
+				IM_CopyQuotedString(pIm->im2.IM_Date, line, sizeof(pIm->im2.IM_Date));
+				break;
+			case IM_Descriptor:
+				IM_CopyQuotedString(pIm->im3.IM_Descriptor, line, sizeof(pIm->im3.IM_Descriptor));
+				break;
+			case IM_Signature:
+				IM_CopyQuotedString(pIm->im4.IM_Signature, line, sizeof(pIm->im4.IM_Signature));
+				break;
+			}
+		}
+	}
+	fclose(pFile);
+}
+
+void IM_DataToStore(const char* pName)
+{
+	FILE* pFile;
+	int sModIdx;
+	pFile = fopen(pName, "w");
+	if (pFile == NULL)
+    {
+		return;
+	}
+
+
+	for (sModIdx = 0; sModIdx < g_device_data.nrOfSubmodules; sModIdx++)
+	{
+		const IM_DATA* pIm = &im_work[sModIdx];
+		if (sModIdx)
+		{
+			fprintf(pFile, "\n");
+		}
+
+		fprintf(pFile, "[SUBMOD%i]\n", sModIdx);
+		fprintf(pFile, "%s = 0x%04x\n", name_IM_PERSISTENT_DATA[IM_Revision_Counter],
+			pIm->im0.IM_Revision_Counter);
+		if (pIm->im0.IM_Supported & 0x02)
+		{
+			fprintf(pFile, "%s = \"%s\"\n", name_IM_PERSISTENT_DATA[IM_Tag_Function],
+				IM_RemTrailBlanks(pIm->im1.IM_Tag_Function, sizeof(pIm->im1.IM_Tag_Function)));
+			fprintf(pFile, "%s = \"%s\"\n", name_IM_PERSISTENT_DATA[IM_Tag_Location],
+				IM_RemTrailBlanks(pIm->im1.IM_Tag_Location, sizeof(pIm->im1.IM_Tag_Location)));
+		}
+		if (pIm->im0.IM_Supported & 0x04)
+		{
+			fprintf(pFile, "%s = \"%s\"\n", name_IM_PERSISTENT_DATA[IM_Date],
+				IM_RemTrailBlanks(pIm->im2.IM_Date, sizeof(pIm->im2.IM_Date)));
+		}
+		if (pIm->im0.IM_Supported & 0x08)
+		{
+			fprintf(pFile, "%s = \"%s\"\n", name_IM_PERSISTENT_DATA[IM_Descriptor],
+				IM_RemTrailBlanks(pIm->im3.IM_Descriptor, sizeof(pIm->im3.IM_Descriptor)));
+		}
+		if (pIm->im0.IM_Supported & 0x10)
+		{
+			fprintf(pFile, "%s = \"%s\"\n", name_IM_PERSISTENT_DATA[IM_Signature],
+				IM_RemTrailBlanks(pIm->im4.IM_Signature, sizeof(pIm->im4.IM_Signature)));
+		}
+	}
+	fclose(pFile);
+}

src/robot_control/src/cp1616/im.h

@@ -0,0 +1,58 @@
+/*------------------------------------------------------------------------*/
+/* Copyright (C) SIEMENS CORP., 2018 All rights reserved.*/
+/* All rights reserved.                                                   */
+/* Redistribution and use in source and binary forms, with or without     */
+/* modification, are permitted provided that the following conditions     */
+/* are met:                                                               */
+/* 1. Redistributions of source code must retain the above copyright      */
+/* notice, this list of conditions and the following disclaimer.          */
+/* 2. Redistributions in binary form must reproduce the above copyright   */
+/* notice, this list of conditions and the following disclaimer in the    */
+/* documentation and/or other materials provided with the distribution.   */
+/* 3. Neither the name of the copyright holder nor the names of its       */
+/* contributors may be used to endorse or promote products derived from   */
+/* this software without specific prior written permission.               */
+/* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS    */
+/* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT      */
+/* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS      */
+/* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE         */
+/* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,   */
+/* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,   */
+/* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS  */
+/* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)                    */
+/* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,    */
+/* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING  */
+/* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE     */
+/* POSSIBILITY OF SUCH DAMAGE.                                            */
+/*------------------------------------------------------------------------*/
+/*------------------------------------------------------------------------*/
+/* Description: This module contains the handling of I&M data records.    */
+/*                                                                        */
+/*------------------------------------------------------------------------*/
+/* Attention : Callbacks are running concurrent in other threads so all   */
+/*             printf statements should be synchronized. But this sample  */
+/*             application doesn't synchronize for simplicity  !          */
+/*------------------------------------------------------------------------*/
+
+#ifndef _IM_H
+#define _IM_H
+
+#include <pniousrd.h>
+
+typedef enum
+{
+	IM0 = 0xAFF0,
+	IM1 = 0xAFF1,
+	IM2 = 0xAFF2,
+	IM3 = 0xAFF3,
+	IM4 = 0xAFF4
+} IM0_idx_e;
+
+void IM_recWrite(PNIO_UINT32 RecordIndex, PNIO_DEV_ADDR Addr, PNIO_UINT32* pBufLen, PNIO_UINT8* pBuf, PNIO_ERR_STAT* pPnioState);
+void IM_RecRead(PNIO_UINT32 RecordIndex, PNIO_DEV_ADDR Addr, PNIO_UINT32* pBufLen, PNIO_UINT8* pBuf, PNIO_ERR_STAT* pPnioState);
+void IM_initImRecords(void);
+void IM_DataFromStore(const char* pName);
+void IM_DataToStore(const char* pName);
+
+
+#endif /* _IM_H */

src/robot_control/src/cp1616/include/os/criticalsection.c

@@ -0,0 +1,93 @@
+/*------------------------------------------------------------------------*/
+/* Copyright (C) SIEMENS CORP., 2018 All rights reserved.*/
+/* All rights reserved.                                                   */
+/* Redistribution and use in source and binary forms, with or without     */
+/* modification, are permitted provided that the following conditions     */
+/* are met:                                                               */
+/* 1. Redistributions of source code must retain the above copyright      */
+/* notice, this list of conditions and the following disclaimer.          */
+/* 2. Redistributions in binary form must reproduce the above copyright   */
+/* notice, this list of conditions and the following disclaimer in the    */
+/* documentation and/or other materials provided with the distribution.   */
+/* 3. Neither the name of the copyright holder nor the names of its       */
+/* contributors may be used to endorse or promote products derived from   */
+/* this software without specific prior written permission.               */
+/* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS    */
+/* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT      */
+/* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS      */
+/* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE         */
+/* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,   */
+/* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,   */
+/* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS  */
+/* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)                    */
+/* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,    */
+/* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING  */
+/* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE     */
+/* POSSIBILITY OF SUCH DAMAGE.                                            */
+/*------------------------------------------------------------------------*/
+/*------------------------------------------------------------------------*/
+/* Attention : Callbacks are running concurrent in other threads so all   */
+/*             printf statements should be synchronized. But this sample  */
+/*             application doesn't synchronize for simplicity  !          */
+/*------------------------------------------------------------------------*/
+
+#include "os/criticalsection.h"
+
+#include <stdlib.h>
+
+#if defined(_WIN32)
+#include <Windows.h>
+#elif defined(__unix)
+#include <pthread.h>
+#endif
+
+typedef struct OsCriticalSection
+{
+#if defined(_WIN32)
+	CRITICAL_SECTION win_cs;
+#elif defined(__unix)
+	pthread_mutex_t lin_cs;
+#endif
+}OsCriticalSection;
+
+/* Initialize the Critical Section */
+void CriticalSectionCreate(CriticalSection* cs)
+{
+	cs->os_cs = (OsCriticalSection*)malloc(sizeof(OsCriticalSection));
+#if defined(_WIN32)
+	InitializeCriticalSection(&cs->os_cs->win_cs);
+#elif defined(__unix)
+	pthread_mutex_init(&cs->os_cs->lin_cs, 0);
+#endif
+}
+
+/* Deinitialize the Critical Section */
+void CriticalSectionDelete(CriticalSection* cs)
+{
+#if defined(_WIN32)
+	DeleteCriticalSection(&cs->os_cs->win_cs);
+#elif defined(__unix)
+	pthread_mutex_destroy(&cs->os_cs->lin_cs);
+#endif
+	free(cs->os_cs);
+}
+
+/* Enter the Critical Section */
+void CriticalSectionEnter(CriticalSection* cs)
+{
+#if defined(_WIN32)
+	EnterCriticalSection(&cs->os_cs->win_cs);
+#elif defined(__unix)
+	pthread_mutex_lock(&cs->os_cs->lin_cs);
+#endif
+}
+
+/* Leave the Critical Section */
+void CriticalSectionLeave(CriticalSection* cs)
+{
+#if defined(_WIN32)
+	LeaveCriticalSection(&cs->os_cs->win_cs);
+#elif defined(__unix)
+	pthread_mutex_unlock(&cs->os_cs->lin_cs);
+#endif
+}

src/robot_control/src/cp1616/include/os/criticalsection.h

@@ -0,0 +1,58 @@
+/*------------------------------------------------------------------------*/
+/* Copyright (C) SIEMENS CORP., 2018 All rights reserved.*/
+/* All rights reserved.                                                   */
+/* Redistribution and use in source and binary forms, with or without     */
+/* modification, are permitted provided that the following conditions     */
+/* are met:                                                               */
+/* 1. Redistributions of source code must retain the above copyright      */
+/* notice, this list of conditions and the following disclaimer.          */
+/* 2. Redistributions in binary form must reproduce the above copyright   */
+/* notice, this list of conditions and the following disclaimer in the    */
+/* documentation and/or other materials provided with the distribution.   */
+/* 3. Neither the name of the copyright holder nor the names of its       */
+/* contributors may be used to endorse or promote products derived from   */
+/* this software without specific prior written permission.               */
+/* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS    */
+/* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT      */
+/* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS      */
+/* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE         */
+/* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,   */
+/* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,   */
+/* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS  */
+/* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)                    */
+/* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,    */
+/* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING  */
+/* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE     */
+/* POSSIBILITY OF SUCH DAMAGE.                                            */
+/*------------------------------------------------------------------------*/
+/*------------------------------------------------------------------------*/
+/* Description: This module hides the os-dependent implementation of      */
+/*              critical sections.                                        */
+/*                                                                        */
+/*------------------------------------------------------------------------*/
+/* Attention : Callbacks are running concurrent in other threads so all   */
+/*             printf statements should be synchronized. But this sample  */
+/*             application doesn't synchronize for simplicity  !          */
+/*------------------------------------------------------------------------*/
+
+#ifndef _CRITICALSECTION_H
+#define _CRITICALSECTION_H
+
+/* encapsulation of os-dependend thread mechanism */
+
+struct OsCriticalSection;
+typedef struct CriticalSection
+{
+	struct OsCriticalSection* os_cs;
+}CriticalSection;
+
+/* Initialize the Critical Section */
+void CriticalSectionCreate(CriticalSection* cs);
+/* Deinitialize the Critical Section */
+void CriticalSectionDelete(CriticalSection* cs);
+/* Enter the Critical Section */
+void CriticalSectionEnter(CriticalSection* cs);
+/* Leave the Critical Section */
+void CriticalSectionLeave(CriticalSection* cs);
+
+#endif /* _CRITICALSECTION_H */

src/robot_control/src/cp1616/include/os/semaphores.c

@@ -0,0 +1,93 @@
+/*------------------------------------------------------------------------*/
+/* Copyright (C) SIEMENS CORP., 2018 All rights reserved.*/
+/* All rights reserved.                                                   */
+/* Redistribution and use in source and binary forms, with or without     */
+/* modification, are permitted provided that the following conditions     */
+/* are met:                                                               */
+/* 1. Redistributions of source code must retain the above copyright      */
+/* notice, this list of conditions and the following disclaimer.          */
+/* 2. Redistributions in binary form must reproduce the above copyright   */
+/* notice, this list of conditions and the following disclaimer in the    */
+/* documentation and/or other materials provided with the distribution.   */
+/* 3. Neither the name of the copyright holder nor the names of its       */
+/* contributors may be used to endorse or promote products derived from   */
+/* this software without specific prior written permission.               */
+/* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS    */
+/* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT      */
+/* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS      */
+/* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE         */
+/* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,   */
+/* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,   */
+/* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS  */
+/* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)                    */
+/* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,    */
+/* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING  */
+/* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE     */
+/* POSSIBILITY OF SUCH DAMAGE.                                            */
+/*------------------------------------------------------------------------*/
+/*------------------------------------------------------------------------*/
+/* Attention : Callbacks are running concurrent in other threads so all   */
+/*             printf statements should be synchronized. But this sample  */
+/*             application doesn't synchronize for simplicity  !          */
+/*------------------------------------------------------------------------*/
+
+#include "os/semaphores.h"
+
+#include <stdlib.h>
+
+#if defined(_WIN32)
+#include <Windows.h>
+#elif defined(__unix)
+#include <semaphore.h>
+#endif
+
+typedef struct OsSemaphore
+{
+#if defined(_WIN32)
+	HANDLE win_sem;
+#elif defined(__unix)
+	sem_t lin_sem;
+#endif
+}OsSemaphore;
+
+/* Initialize the semaphore */
+void SemaphoreCreate(Semaphore* sem, int value)
+{
+	sem->os_sem = (OsSemaphore*)malloc(sizeof(OsSemaphore));
+#if defined(_WIN32)
+	sem->os_sem->win_sem = CreateSemaphore(0, value, LONG_MAX, 0);
+#elif defined(__unix)
+	sem_init(&sem->os_sem->lin_sem, 0, value);
+#endif
+}
+
+/* Deinitialize the semaphore */
+void SemaphoreDelete(Semaphore* sem)
+{
+#if defined(_WIN32)
+	CloseHandle(sem->os_sem->win_sem);
+#elif defined(__unix)
+	sem_destroy(&sem->os_sem->lin_sem);
+#endif
+}
+
+/* Signal (increment) the semaphore */
+void SemaphoreSignal(Semaphore* sem)
+{
+#if defined(_WIN32)
+	ReleaseSemaphore(sem->os_sem->win_sem, 1, 0);
+#elif defined(__unix)
+	sem_post(&sem->os_sem->lin_sem);
+#endif
+}
+
+/* Wait for the semaphore (and decrement it) */
+void SemaphoreWait(Semaphore* sem)
+{
+#if defined(_WIN32)
+	WaitForSingleObject(sem->os_sem->win_sem, INFINITE);
+#elif defined(__unix)
+	sem_wait(&sem->os_sem->lin_sem);
+#endif
+}
+

src/robot_control/src/cp1616/include/os/semaphores.h

@@ -0,0 +1,58 @@
+/*------------------------------------------------------------------------*/
+/* Copyright (C) SIEMENS CORP., 2018 All rights reserved.*/
+/* All rights reserved.                                                   */
+/* Redistribution and use in source and binary forms, with or without     */
+/* modification, are permitted provided that the following conditions     */
+/* are met:                                                               */
+/* 1. Redistributions of source code must retain the above copyright      */
+/* notice, this list of conditions and the following disclaimer.          */
+/* 2. Redistributions in binary form must reproduce the above copyright   */
+/* notice, this list of conditions and the following disclaimer in the    */
+/* documentation and/or other materials provided with the distribution.   */
+/* 3. Neither the name of the copyright holder nor the names of its       */
+/* contributors may be used to endorse or promote products derived from   */
+/* this software without specific prior written permission.               */
+/* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS    */
+/* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT      */
+/* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS      */
+/* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE         */
+/* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,   */
+/* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,   */
+/* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS  */
+/* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)                    */
+/* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,    */
+/* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING  */
+/* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE     */
+/* POSSIBILITY OF SUCH DAMAGE.                                            */
+/*------------------------------------------------------------------------*/
+/*------------------------------------------------------------------------*/
+/* Description: This module hides the os-dependent implementation of      */
+/*              semaphores.                                               */
+/*                                                                        */
+/*------------------------------------------------------------------------*/
+/* Attention : Callbacks are running concurrent in other threads so all   */
+/*             printf statements should be synchronized. But this sample  */
+/*             application doesn't synchronize for simplicity  !          */
+/*------------------------------------------------------------------------*/
+
+#ifndef _SEMAPHORES_H
+#define _SEMAPHORES_H
+
+/* encapsulation of os-dependend semaphore mechanism */
+
+struct OsSemaphore;
+typedef struct Semaphore
+{
+	struct OsSemaphore* os_sem;
+}Semaphore; 
+
+/* Initialize the semaphore with a initial value */
+void SemaphoreCreate(Semaphore* sem, int value);
+/* Deinitialize the semaphore */
+void SemaphoreDelete(Semaphore* sem);
+/* Signal (increment) the semaphore */
+void SemaphoreSignal(Semaphore* sem);
+/* Wait for the semaphore (and decrement it) */
+void SemaphoreWait(Semaphore* sem);
+
+#endif /* _SEMAPHORES_H */

src/robot_control/src/cp1616/include/os/thread.c

@@ -0,0 +1,131 @@
+/*------------------------------------------------------------------------*/
+/* Copyright (C) SIEMENS CORP., 2018 All rights reserved.*/
+/* All rights reserved.                                                   */
+/* Redistribution and use in source and binary forms, with or without     */
+/* modification, are permitted provided that the following conditions     */
+/* are met:                                                               */
+/* 1. Redistributions of source code must retain the above copyright      */
+/* notice, this list of conditions and the following disclaimer.          */
+/* 2. Redistributions in binary form must reproduce the above copyright   */
+/* notice, this list of conditions and the following disclaimer in the    */
+/* documentation and/or other materials provided with the distribution.   */
+/* 3. Neither the name of the copyright holder nor the names of its       */
+/* contributors may be used to endorse or promote products derived from   */
+/* this software without specific prior written permission.               */
+/* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS    */
+/* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT      */
+/* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS      */
+/* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE         */
+/* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,   */
+/* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,   */
+/* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS  */
+/* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)                    */
+/* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,    */
+/* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING  */
+/* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE     */
+/* POSSIBILITY OF SUCH DAMAGE.                                            */
+/*------------------------------------------------------------------------*/
+/*------------------------------------------------------------------------*/
+/* Attention : Callbacks are running concurrent in other threads so all   */
+/*             printf statements should be synchronized. But this sample  */
+/*             application doesn't synchronize for simplicity  !          */
+/*------------------------------------------------------------------------*/
+
+#include "os/thread.h"
+
+#include <stdlib.h>
+
+#if defined(_WIN32)
+#include <Windows.h>
+#elif defined(__unix)
+#include <pthread.h>
+#endif
+
+typedef struct OsThread
+{
+#if defined(_WIN32)
+	HANDLE win_thread;
+#elif defined(__unix)
+	pthread_t lin_thread;
+#endif
+}OsThread;
+
+typedef struct
+{
+	ThreadRoutine pFunc;
+	void* pParam;
+}ThreadData;
+
+/* os dependent wrapper function */
+#if defined(_WIN32)
+DWORD _stdcall threadFunc(void* pParam)
+#elif defined(__unix)
+void* threadFunc(void* pParam)
+#endif
+{
+	ThreadData* tempData = (ThreadData*)pParam;
+	ThreadRoutine pFunc = tempData->pFunc;
+	void* pRemoteParam = tempData->pParam;
+
+	free(tempData);
+
+	pFunc(pRemoteParam);
+
+	return 0;
+}
+
+/* start a thread */
+void ThreadStart(Thread* thread, ThreadRoutine pFunc, void* pParam)
+{
+#if defined(_WIN32)
+	HANDLE thread_handle;
+#elif defined(__unix)
+	pthread_t thread_id;
+#endif
+
+	if (pFunc != 0)
+	{
+		ThreadData* data = (ThreadData*)malloc(sizeof(ThreadData));
+		data->pFunc = pFunc;
+		data->pParam = pParam;
+
+#if defined(_WIN32)
+		thread_handle = CreateThread(0, 0, threadFunc, data, 0, 0);
+		if (thread != 0)
+		{
+			thread->os_thread = (OsThread*)malloc(sizeof(OsThread*));
+			thread->os_thread->win_thread = thread_handle;
+		}
+		else
+		{
+			CloseHandle(thread_handle);
+		}
+#elif defined(__unix)
+		pthread_create(&thread_id, 0, threadFunc, data);
+		if (thread != 0)
+		{
+			thread->os_thread = (OsThread*)malloc(sizeof(OsThread*));
+			thread->os_thread->lin_thread = thread_id;
+		}
+#endif
+	}
+}
+
+/* wait for a thread to finish */
+void ThreadJoin(Thread* thread)
+{
+#if defined(_WIN32)
+	WaitForSingleObject(thread->os_thread->win_thread, INFINITE);
+#elif defined(__unix)
+	pthread_join(thread->os_thread->lin_thread, 0);
+#endif
+}
+
+/* delete a thread object (does not end the thread) */
+void ThreadFree(Thread* thread)
+{
+#if defined(_WIN32)
+	CloseHandle(thread->os_thread->win_thread);
+#endif
+	free(thread->os_thread);
+}

src/robot_control/src/cp1616/include/os/thread.h

@@ -0,0 +1,59 @@
+/*------------------------------------------------------------------------*/
+/* Copyright (C) SIEMENS CORP., 2018 All rights reserved.*/
+/* All rights reserved.                                                   */
+/* Redistribution and use in source and binary forms, with or without     */
+/* modification, are permitted provided that the following conditions     */
+/* are met:                                                               */
+/* 1. Redistributions of source code must retain the above copyright      */
+/* notice, this list of conditions and the following disclaimer.          */
+/* 2. Redistributions in binary form must reproduce the above copyright   */
+/* notice, this list of conditions and the following disclaimer in the    */
+/* documentation and/or other materials provided with the distribution.   */
+/* 3. Neither the name of the copyright holder nor the names of its       */
+/* contributors may be used to endorse or promote products derived from   */
+/* this software without specific prior written permission.               */
+/* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS    */
+/* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT      */
+/* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS      */
+/* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE         */
+/* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,   */
+/* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,   */
+/* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS  */
+/* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)                    */
+/* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,    */
+/* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING  */
+/* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE     */
+/* POSSIBILITY OF SUCH DAMAGE.                                            */
+/*------------------------------------------------------------------------*/
+/*------------------------------------------------------------------------*/
+/* Description: This module hides the os-dependent implementation to      */
+/*              start a thread.                                           */
+/*                                                                        */
+/*------------------------------------------------------------------------*/
+/* Attention : Callbacks are running concurrent in other threads so all   */
+/*             printf statements should be synchronized. But this sample  */
+/*             application doesn't synchronize for simplicity  !          */
+/*------------------------------------------------------------------------*/
+
+#ifndef _THREAD_H
+#define _THREAD_H
+
+/* encapsulation of os-dependend thread mechanism */
+
+struct OsThread;
+typedef struct Thread
+{
+	struct OsThread* os_thread;
+}Thread;
+
+/* thread implementation prototype */
+typedef void (*ThreadRoutine)(void* pParam);
+/* start a thread */
+void ThreadStart(Thread* thread, ThreadRoutine pFunc, void* pParam);
+/* wait for a thread to finish */
+void ThreadJoin(Thread* thread);
+/* delete a thread object (does not end the thread) */
+void ThreadFree(Thread* thread);
+
+
+#endif /* _THREAD_H */

src/robot_control/src/cp1616/pe.h

@@ -0,0 +1,62 @@
+/*------------------------------------------------------------------------*/
+/* Copyright (C) SIEMENS CORP., 2018 All rights reserved.*/
+/* All rights reserved.                                                   */
+/* Redistribution and use in source and binary forms, with or without     */
+/* modification, are permitted provided that the following conditions     */
+/* are met:                                                               */
+/* 1. Redistributions of source code must retain the above copyright      */
+/* notice, this list of conditions and the following disclaimer.          */
+/* 2. Redistributions in binary form must reproduce the above copyright   */
+/* notice, this list of conditions and the following disclaimer in the    */
+/* documentation and/or other materials provided with the distribution.   */
+/* 3. Neither the name of the copyright holder nor the names of its       */
+/* contributors may be used to endorse or promote products derived from   */
+/* this software without specific prior written permission.               */
+/* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS    */
+/* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT      */
+/* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS      */
+/* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE         */
+/* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,   */
+/* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,   */
+/* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS  */
+/* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)                    */
+/* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,    */
+/* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING  */
+/* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE     */
+/* POSSIBILITY OF SUCH DAMAGE.                                            */
+/*------------------------------------------------------------------------*/
+/*------------------------------------------------------------------------*/
+/* Description: This module contains the implementation for PROFI ENERGY. */
+/*                                                                        */
+/*------------------------------------------------------------------------*/
+/* Attention : Callbacks are running concurrent in other threads so all   */
+/*             printf statements should be synchronized. But this sample  */
+/*             application doesn't synchronize for simplicity  !          */
+/*------------------------------------------------------------------------*/
+
+#ifndef _PE_H
+#define _PE_H
+
+#include <pniousrd.h>
+
+extern int g_DO_PE_STARTUP;
+extern int g_DO_PE_SHUTDOWN;
+extern int g_stopreq;
+extern int g_shutdown_host;
+
+#ifndef PNIO_OK_PE_WAKE_ONLINE
+#define PNIO_OK_PE_WAKE_ONLINE 0x00000001
+#endif
+#ifndef PNIO_OK_PE_WAKE_OFFLINE
+#define PNIO_OK_PE_WAKE_OFFLINE 0x00000002
+#endif
+#ifndef PNIO_CEP_PE_MODE_ENABLE
+#define PNIO_CEP_PE_MODE_ENABLE 0x00000020
+#endif
+
+void PNIO_cbf_rec_write_pe(PNIO_UINT32 DevHndl, PNIO_UINT32 Api, PNIO_UINT16 SessionKey, PNIO_UINT32 SequenceNum, PNIO_DEV_ADDR* pAddr, PNIO_UINT32 RecordIndex, PNIO_UINT32* pBufLen, PNIO_UINT8* pBuffer, PNIO_ERR_STAT* pPnioState);
+void PNIO_cbf_rec_read_pe( PNIO_UINT32 DevHndl, PNIO_UINT32 Api, PNIO_UINT16 SessionKey, PNIO_UINT32 SequenceNum, PNIO_DEV_ADDR* pAddr, PNIO_UINT32 RecordIndex, PNIO_UINT32* pBufLen, PNIO_UINT8* pBuffer, PNIO_ERR_STAT* pPnioState);
+PNIO_UINT32  PE_Initialize(void);
+void PE_print_error(char* prefix);
+
+#endif /* _PE_H */