modbus_linux with test

.gitignore

@@ -0,0 +1,34 @@
+
+#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*/
+[Bb]uild*/
+*.lib
+*.sbr
+obj/
+[Rr]elease*/
+_ReSharper*/
+[Tt]est[Rr]esult*
+.vs/
+.vscode/
+#Nuget packages folder
+packages/

CMakeLists.txt

@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(plc_proc)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 
+set(CMAKE_INCLUDE_CURRENT_DIR ON) 
+set(CMAKE_BUILD_TYPE Debug)
+
+include_directories ("/home/youchen/Documents/modbus_test/include/") 
+include_directories ("/usr/local/include/modbus/") 
+include_directories ("/opt/ros/melodic/include/") 
+include_directories ("/usr/include/eigen3/") 
+
+link_directories ("/usr/local/lib") 
+
+set(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/libs) 
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/libs) 
+
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
+#add_executable(plc_proc src/PLCProcess.cpp src/PLCModbus.cpp)
+add_executable(plc_proc src/plc_test.cpp src/PLCModbus.cpp)
+target_link_libraries(plc_proc modbus)

PLCMonitor.cpp

@@ -0,0 +1,472 @@
+#include "stdafx.h"
+#include "Automatic.h"
+#include "PLCMonitor.h"
+
+namespace modbus
+{
+	const clock_t CPLCMonitor::PLC_LASER_TIMEOUT_READY = 10000;		//����źŵ�������ʱ
+	const clock_t CPLCMonitor::PLC_LASER_TIMEOUT_PINGPANG = 3000;
+	const clock_t CPLCMonitor::PLC_LASER_TIMEOUT_START = 300;
+	const clock_t CPLCMonitor::PLC_LASER_TIMEOUT_MAXMEASURE = 600000;
+
+	CPLCMonitor::CPLCMonitor(void* pOwnerObject)
+		:_monitoring(FALSE)
+		, _heartbeat_write_clock(clock())
+		, m_lockValue(0)
+		, m_lock_cmd_Value(0)
+		, m_callback(0)
+		,m_pointer(0)
+		, m_logWnd(0)
+		,m_laser_data_wnd(0)
+    {
+        cs_pcl.Initlock(&m_lockValue);
+        cs_cmd.Initlock(&m_lock_cmd_Value);
+        memset(&_value, 0, sizeof(_value));
+    }
+
+    CPLCMonitor::~CPLCMonitor()
+    {
+		_monitoring = false;
+		if (m_thread_read)
+		{
+			WaitForSingleObject(m_thread_read->m_hThread, INFINITE);
+			//CloseHandle(m_pThread);
+			m_thread_read = NULL;
+		}
+    }
+
+    int CPLCMonitor::connect(const char *ip, int port, int slave_id)
+    {
+        cs_pcl.Lock();
+        if (-1 == dev.initialize(ip, port, slave_id))
+        {
+            cs_pcl.Unlock();
+            return -1;
+        }
+        cs_pcl.Unlock();
+
+        _monitoring = TRUE;
+        if (FALSE == Start())
+        {
+            dev.deinitialize();
+            return -1;
+        }
+		m_thread_read = AfxBeginThread(thread_monitor, this);
+		m_strIP = ip;
+		m_port = port;
+		m_slave_id = slave_id;
+
+        return 0;
+    }
+
+	void CPLCMonitor::set_callback(CommandCallback func, void* pointer)
+	{
+		m_callback = func;
+		m_pointer = pointer;
+	}
+
+	int CPLCMonitor::reconnect()
+	{
+		cs_pcl.Lock();
+		if (-1 == dev.initialize(m_strIP.c_str(), m_port, m_slave_id))
+		{
+			cs_pcl.Unlock();
+			return -1;
+		}
+		cs_pcl.Unlock();
+		return 0;
+	}
+
+    void CPLCMonitor::disconnect()
+    {
+        _monitoring = FALSE;
+        Join(1000);
+        Terminate();
+
+        cs_pcl.Lock();
+        dev.deinitialize();
+        cs_pcl.Unlock();
+    }
+
+    int CPLCMonitor::read_registers(int addr, int nb, uint16_t *dest)
+    {
+        cs_pcl.Lock();
+        int rc = dev.read_registers(addr, nb, dest);
+        cs_pcl.Unlock();
+		if (0 != rc)
+		{
+			reconnect();//�˿�������
+		}
+        return rc;
+    }
+
+    int CPLCMonitor::write_registers(int addr, int nb, uint16_t *dest)
+    {
+        cs_pcl.Lock();
+        int rc = dev.write_registers(addr, nb, dest);
+        cs_pcl.Unlock();
+
+        return rc;
+    }
+
+    int CPLCMonitor::read_register(int addr, uint16_t *dest)
+    {
+        cs_pcl.Lock();
+        int rc = dev.read_register(addr, dest);
+        cs_pcl.Unlock();
+        return rc;
+    }
+
+    int CPLCMonitor::write_register(int addr, uint16_t *dest)
+    {
+        cs_pcl.Lock();
+        int rc = dev.write_register(addr, dest);
+        cs_pcl.Unlock();
+
+        return rc;
+    }
+
+    int CPLCMonitor::clear_laserstatus_register(uint16_t value)
+    {
+        cs_pcl.Lock();
+        int rc = dev.write_register(REG_WHISKBOOMLASER_STATUS, &value);
+        cs_pcl.Unlock();
+        _heartbeat_write_clock = clock();
+
+        return rc;
+    }
+
+    int CPLCMonitor::write_laserstatus_register(uint16_t value)
+    {
+        uint16_t dest = 0xff;
+        cs_pcl.Lock();
+
+        int rc = dev.read_register(REG_WHISKBOOMLASER_STATUS, &dest);
+        if (PLC_LASER_ERROR != dest)
+        {
+            rc = dev.write_register(REG_WHISKBOOMLASER_STATUS, &value);
+        }
+        cs_pcl.Unlock();
+
+        _heartbeat_write_clock = clock();
+
+		if (0 != rc)
+		{
+			reconnect();//�˿�������
+		}
+
+        return rc;
+    }
+
+    int CPLCMonitor::write_laserresult_register(int addr,uint16_t *pvalue)
+    {
+        uint16_t dest = 0xff;
+        cs_pcl.Lock();
+
+		int rc= dev.write_registers(addr, 7, pvalue);
+        /*int rc = dev.read_register(REG_WHISKBOOMLASER_STATUS, &dest);
+        if (PLC_LASER_ERROR != dest)
+        {
+            rc = dev.write_registers(REG_WHISKBOOMLASER_STATUS, 7, pvalue);
+        }*/
+        cs_pcl.Unlock();
+
+        _heartbeat_write_clock = clock();
+
+        return rc;
+    }
+
+	void CPLCMonitor::MeasureComplete(bool bOK)
+	{
+		//_measure_finished = TRUE;
+		if (!bOK)
+		{
+			write_laserstatus_register(PLC_LASER_FINISH_FAILED);
+		}
+		else
+		{
+			write_laserstatus_register(PLC_LASER_FINISH_OK);
+		}
+	}
+
+    bool CPLCMonitor::setMeasureResult(int addr, struct whiskboom_laser_value * p)
+    {
+		if (p == 0||addr<0)	//ʧ�ܣ�û�а�ɨ�����Ӳ������
+		{
+			return false;
+		}
+		else
+		{
+			uint16_t response[7] = { 0 };
+			memset(response, 0, sizeof(uint16_t) * 7);
+			if (p->corrected)
+			{
+				response[6] = eLaser_data_ok;
+			}
+			else {
+				response[6] = eLaser_data_failed;
+			}
+			response[0] = p->x;
+			response[1] = p->y;
+			response[2] = (int(p->a) % 18000);
+			response[3] = p->l;
+			response[4] = p->w;
+			response[5] = p->h;
+			write_laserresult_register(addr,(uint16_t *)response);
+			return true;
+		}
+    }
+
+	void CPLCMonitor::Monitoring()
+	{
+		const int SIGNAL_NUM = 44;
+		const int SIGNAL_ADDR = 0;
+		static uint16_t value[SIGNAL_NUM] = { 0 };
+		static uint16_t last_value[SIGNAL_NUM] = { 0 };
+		int rc = read_registers(SIGNAL_ADDR, SIGNAL_NUM, last_value);
+		LOG(INFO) << "\t����signal����߳�";
+		while (_monitoring)
+		{
+			memset(value, 0, SIGNAL_NUM * sizeof(uint16_t));
+			int rc = read_registers(SIGNAL_ADDR, SIGNAL_NUM, value);
+			
+			if (rc == 0)
+			{
+				for (int i = REG_PARKSTATUS; i < SIGNAL_NUM; ++i)
+				{
+					if (value[i] != last_value[i])
+					{
+						if (m_logWnd != NULL)
+							::PostMessage(m_logWnd->m_hWnd, WM_LOG_MSG, (WPARAM)(i + SIGNAL_ADDR), (LPARAM)(value[i]));		
+					}
+				}
+				memcpy(last_value, value, sizeof(uint16_t)*SIGNAL_NUM);
+			}
+			if (m_logWnd != NULL)
+				::PostMessage(m_logWnd->m_hWnd, WM_MONITOR_MSG, (WPARAM)SIGNAL_NUM, (LPARAM)value);
+			if (m_laser_data_wnd)
+				::PostMessage(m_laser_data_wnd->m_hWnd, WM_MONITOR_MSG, (WPARAM)(SIGNAL_NUM), (LPARAM)(value));
+			Sleep(100);
+		}
+		LOG(INFO) << "\t����signal����߳�";
+	}
+
+	UINT CPLCMonitor::thread_monitor(LPVOID lp)
+	{
+		CPLCMonitor* plc = (CPLCMonitor*)lp;
+		plc->Monitoring();
+		return 0;
+	}
+
+    void CPLCMonitor::Run()
+    {
+        uint16_t value[2] = { 0xffff,0xffff };
+        uint16_t parking_status = 0xff;
+        uint16_t response[7] = { 0 };
+        int rc = -1;
+        clock_t current_clock;
+
+        uint16_t finite_state_machines = PLC_LASER_READY;
+        write_laserstatus_register(finite_state_machines);
+        uint16_t las_parkstatus = 0;
+
+        while (_monitoring)
+        {
+            memset(value, 0xff, sizeof(value));
+
+            rc = read_registers(REG_PARKSTATUS, 2, value);
+            if (-1 == rc)
+            {
+                fprintf(stderr, "CPLCMonitor: read regiser failed\n");
+            }
+
+            parking_status = value[0];
+            finite_state_machines = value[1];
+
+            //PLC ��Ҫͣ���ȴ�״̬
+            if (PLC_PARKING_WAIT == parking_status)
+            {
+                current_clock = clock();
+
+                //�����ϴ�״̬Ϊ��������ɳɹ����߲������ʧ�ܵ�״̬
+                if (PLC_LASER_FINISH_OK == finite_state_machines
+                    || PLC_LASER_FINISH_FAILED == finite_state_machines)
+                {
+                    //����10�룬��ʼ���ؾ���״̬
+                    if (current_clock - _heartbeat_write_clock >= PLC_LASER_TIMEOUT_READY)
+                    {
+						//���������
+						if (PLC_LASER_FINISH_FAILED == finite_state_machines)
+						{
+							/*memset(response, 0, 7 * sizeof(uint16_t));
+							write_laserresult_register((uint16_t *)response);*/
+						}
+                        write_laserstatus_register(PLC_LASER_READY);
+						finite_state_machines = PLC_LASER_READY;
+                    }
+                }
+                //�����ϴ�״̬Ϊ��������������״̬1��
+                else if (PLC_LASER_READY == finite_state_machines
+                    || PLC_LASER_PONG == finite_state_machines)
+                {
+                    //����3�룬��ʼ��������״̬2
+                    if (current_clock - _heartbeat_write_clock >= PLC_LASER_TIMEOUT_PINGPANG)
+                    {
+                        write_laserstatus_register(PLC_LASER_PING);
+						finite_state_machines = PLC_LASER_PING;
+                    }
+                }
+                //�����ϴ�״̬Ϊ������״̬2
+                else if (PLC_LASER_PING == finite_state_machines)
+                {
+                    //����3�룬��ʼ��������״̬1
+                    if (current_clock - _heartbeat_write_clock >= PLC_LASER_TIMEOUT_PINGPANG)
+                    {
+                        write_laserstatus_register(PLC_LASER_PONG);
+						finite_state_machines = PLC_LASER_PONG;
+                    }
+                }
+                //�����ϴ�״̬Ϊ��������ʼ�������У�
+                else if (PLC_LASER_START == finite_state_machines
+                    || PLC_LASER_WORKING == finite_state_machines)
+                {
+                    //��ʱ��������ڲ���״̬��,�ü������ֹͣ�������
+                    //    1. ���δ�ڲ���״̬���������״̬�������κη���ֵ��
+                    //    2. ����ڲ���״̬���в���ʧ����Ϣ�봦��
+					LOG(ERROR) << "\t �����п�ʼ�ź���ֹ";
+                   /* cs_cmd.Lock();
+					for (std::vector<CLaserNet*>::iterator it = _cmdlistLaserPointer.begin(); it != _cmdlistLaserPointer.end(); ++it)
+						(*it)->Stop();
+					cs_cmd.Unlock();*/
+
+                    write_laserstatus_register(PLC_LASER_READY);
+					finite_state_machines = PLC_LASER_READY;
+                }
+                //�����޸����󣬵�ϵͳ����
+                else if (PLC_LASER_ERROR == finite_state_machines)
+                {
+                    fprintf(stderr, "\n\n\n\n\n\n�����޸����󣬵�ϵͳ����\n\n\n\n\n\n");
+                }
+                else
+                {
+                    fprintf(stderr, "\n\n\n\n\n\n&&&&&&&&&!!!!!!!!!!!!!@@@@@@@@@@@@@@$$$$$$$$$$$$$$$$$$$��ȫ�����ܳ��ֵ�״̬\n\n\n\n\n\n");
+                }
+                las_parkstatus = PLC_PARKING_WAIT;
+
+            }
+            //PLC ��Ҫͣ��������ʼ
+            else if (parking_status>0)
+            {
+                //�����ϴ�״̬Ϊ������������״̬1������״̬2
+                if(PLC_LASER_READY == finite_state_machines
+                        || PLC_LASER_PING == finite_state_machines
+                        || PLC_LASER_PONG == finite_state_machines
+                        || PLC_LASER_FINISH_OK == finite_state_machines
+                        || PLC_LASER_FINISH_FAILED == finite_state_machines)
+                {
+                    if (PLC_PARKING_WAIT == las_parkstatus)
+                        //�ò������Ϊδ���״̬
+                    {
+                        _measure_finished = FALSE;
+						
+                        //������ʼ
+						if (m_callback)
+						{
+							//char plate = parking_status;
+							
+							uint16_t param = 0x03;
+							m_callback(true, param, m_pointer);
+						}
+
+                        //���ز�����ʼ״̬
+                        write_laserstatus_register(PLC_LASER_START);
+						finite_state_machines = PLC_LASER_START;
+                    }
+                }
+				//�ϴ�״̬Ϊ��������ɳɹ���������ɴ���
+				if (PLC_LASER_FINISH_OK == finite_state_machines
+					|| PLC_LASER_FINISH_FAILED == finite_state_machines)
+				{
+					if (current_clock - _heartbeat_write_clock >= PLC_LASER_TIMEOUT_PINGPANG)
+					{
+						write_laserstatus_register(PLC_LASER_PING);
+						finite_state_machines = PLC_LASER_PING;
+					}
+				}
+
+                //�����ϴ�״̬Ϊ:��ʼ����
+                else if (PLC_LASER_START == finite_state_machines)
+                {
+					current_clock = clock();  
+                    //����200���룬���ز�����
+                    if (current_clock - _heartbeat_write_clock >= PLC_LASER_TIMEOUT_START)
+                    {
+                        write_laserstatus_register(PLC_LASER_WORKING);
+						finite_state_machines = PLC_LASER_WORKING;
+                    }
+                }
+                else if (PLC_LASER_WORKING == finite_state_machines)
+                {
+                    if (TRUE == _measure_finished)
+                    {
+						//�������
+                       /* memset(response, 0, sizeof(uint16_t) * 7);
+                        if (_value.corrected)
+                        {
+                            response[0] = PLC_LASER_FINISH_OK;
+                        }
+                        else {
+                            response[0] = PLC_LASER_FINISH_FAILED;
+                        }
+						response[1] = _value.x;
+						response[2] = _value.y;
+						response[3] = (int(_value.a) % 18000);
+						response[4] = _value.l;
+						response[5] = _value.w;
+						response[6] = _value.h;
+                        write_laserresult_register((uint16_t *)response);*/
+						write_laserstatus_register(PLC_LASER_FINISH_FAILED);
+						finite_state_machines = PLC_LASER_FINISH_FAILED;
+                    }
+                    else {
+                        current_clock = clock();
+                        if (current_clock - _heartbeat_write_clock > PLC_LASER_TIMEOUT_MAXMEASURE) //10���ӻ�û���ز�������� ˵��ϵͳ���ܳ�����
+                        {
+                            write_laserstatus_register(PLC_LASER_ERROR);
+							finite_state_machines = PLC_LASER_ERROR;
+                        }
+                    }
+                }
+                else if (PLC_LASER_ERROR == finite_state_machines)
+                {
+
+                    fprintf(stderr, "\n\n\n\n\n\n�����޸����󣬵�ϵͳ����\n\n\n\n\n\n");
+                }
+                else
+                {
+                    fprintf(stderr, "\n\n\n\n\n\n&&&&&&&&&!!!!!!!!!!!!!@@@@@@@@@@@@@@$$$$$$$$$$$$$$$$$$$��ȫ�����ܳ��ֵ�״̬\n\n\n\n\n\n");
+                }
+
+                las_parkstatus = PCL_PARKING_REQUEST;
+            }
+            else
+            {
+				if (PCL_PARKING_ERROR != las_parkstatus)
+                {
+                    LOG(ERROR)<<("\t���ؾ���:PLC��Ҫͣ��״̬�쳣�������޸����󣬵�ϵͳ����");
+					//ֹͣ���м���
+					if (m_callback)
+					{
+						char laser = 0xFF;
+						m_callback(false, uint16_t(laser), m_pointer);
+					}
+                    write_laserstatus_register(PLC_LASER_READY);
+					finite_state_machines = PLC_LASER_READY;
+                }
+                Sleep(200);
+				las_parkstatus = PCL_PARKING_ERROR;
+            }
+            Sleep(50);
+        }
+    }
+}

PLCMonitor.h

@@ -0,0 +1,141 @@
+#pragma once
+
+#include "LibmodbusWrapper.h"
+#include "Runnable.h"
+//#include "CriticalSection.h"
+#include "Spinlock.h"
+#include <vector>
+#include <time.h>
+
+struct whiskboom_laser_value
+{
+    float x;
+    float y;
+    float a;
+    float l;
+    float w;
+    float h;
+    BOOL corrected;
+};
+
+#define WM_LOG_MSG						WM_USER+1001
+#define WM_MONITOR_MSG					WM_USER+1002
+
+namespace modbus
+{
+#define REG_PARKSTATUS (42)
+#define REG_WHISKBOOMLASER_STATUS (43)
+#define REG_WHISKBOOMLASER_CENTERX (2)
+#define REG_WHISKBOOMLASER_CENTERY (3)
+#define REG_WHISKBOOMLASER_ANGLEA (4)
+#define REG_WHISKBOOMLASER_LENGTH (5)
+#define REG_WHISKBOOMLASER_WIDTH (6)
+#define REG_WHISKBOOMLASER_HEIGHT (7)
+
+    enum PLC_PARKING_STATUS
+    {
+        PLC_PARKING_WAIT = 0,
+        PCL_PARKING_REQUEST,
+        PCL_PARKING_ERROR
+    };
+
+	enum LASER_DATA_CORRECT
+	{
+		eLaser_data_init=0,
+		eLaser_data_ok,
+		eLaser_data_failed
+	};
+
+    enum PCL_LASER_STATUS
+    {
+        PLC_LASER_READY = 0,
+        PLC_LASER_START,
+        PLC_LASER_WORKING,
+        PLC_LASER_FINISH_OK,
+        PLC_LASER_FINISH_FAILED,
+        PLC_LASER_ERROR,
+        PLC_LASER_PING=254,
+        PLC_LASER_PONG
+    };
+
+
+	typedef void(*CommandCallback)(bool On, uint16_t param,void* pointer);
+
+    class CPLCMonitor :
+        public CRunnable
+    {
+    public:
+        CPLCMonitor(void* pOwnerObject=NULL);
+        ~CPLCMonitor();
+    public:
+        int connect(const char *ip, int port, int slave_id);
+        void disconnect();
+		int reconnect();
+		void set_callback(CommandCallback func, void* pointer);
+    private:
+        SpinLock cs_cmd;
+        volatile LONG m_lock_cmd_Value;
+    private:
+        BOOL _measure_finished;
+        struct whiskboom_laser_value _value;
+    public:
+       // void setMeasureResult(BOOL corrected = FALSE, struct whiskboom_laser_value * value = NULL);
+		bool setMeasureResult(int addr, struct whiskboom_laser_value * value = NULL);
+		void MeasureComplete(bool bOK=true);
+	public:
+		int write_laserstatus_register(uint16_t value);
+	protected:
+		CommandCallback		m_callback;
+		void*				m_pointer;
+
+    public:
+        BOOL isMonitor() { return _monitoring == TRUE; };
+    private:
+        BOOL _monitoring;
+
+    private:
+        CLibmodbusWrapper dev;
+    
+    private:
+        clock_t _heartbeat_write_clock;
+
+    private:
+        int read_registers(int addr, int nb, uint16_t *dest);
+        int write_registers(int addr, int nb, uint16_t *dest);
+
+        int read_register(int addr, uint16_t *dest);
+        int write_register(int addr, uint16_t *dest);
+
+        int clear_laserstatus_register(uint16_t value);
+        int write_laserresult_register(int addr ,uint16_t *pvalue);
+
+    private:
+        static const clock_t PLC_LASER_TIMEOUT_READY;
+		static const clock_t PLC_LASER_TIMEOUT_PINGPANG;
+		static const clock_t PLC_LASER_TIMEOUT_START;
+		static const clock_t PLC_LASER_TIMEOUT_MAXMEASURE;
+
+    public:
+        virtual void Run();
+		void		SetLogWnd(CWnd* wnd,CWnd* laser_wnd=0) { 
+			m_logWnd = wnd; 
+			m_laser_data_wnd = laser_wnd;
+		}
+	private:
+		void Monitoring();
+		static UINT thread_monitor(LPVOID lp);
+		CWnd*						m_logWnd;
+		CWnd*						m_laser_data_wnd;
+		CWinThread*						m_thread_read;
+    private:
+        //CCriticalSection cs;
+        SpinLock cs_pcl;
+        volatile LONG m_lockValue;
+
+		std::string				m_strIP;
+		int						m_port;
+		int						m_slave_id;
+
+    };
+}
+

include/PLCModbus.h

@@ -0,0 +1,54 @@
+
+#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>
+
+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)
+
+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;
+
+private:
+    };
+};
+#endif

include/PLCProcess.h

@@ -0,0 +1,45 @@
+#ifndef __PLC_PROCESS__HH__
+#define __PLC_PROCESS__HH__
+
+#include <tf/tf.h>
+#include "tf2_msgs/TFMessage.h"
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <string>
+#include <boost/foreach.hpp>
+#include <boost/thread.hpp>
+#include <vector>
+#include <math.h>
+#include "PLCModbus.h"
+
+class PLCProcess
+{
+public:
+    PLCProcess();
+    ~PLCProcess();
+    void Update(std::vector<std::vector<tf::Transform>> trajectorys);
+    struct TRAJECTORY
+    {
+        std::vector<std::vector<tf::Transform>> left_trajectorys_;
+        std::vector<std::vector<tf::Transform>> right_trajectorys_;
+        bool bUpdate;
+
+    };
+
+protected:
+    bool split_trajectorys(std::vector<std::vector<tf::Transform>> trajectorys);
+
+    bool send_trajectory(std::vector<tf::Transform> trajectory_l,std::vector<tf::Transform> trajectory_r);
+    std::vector<uint16_t> absolute_convert_to_relative(std::vector<tf::Transform> tf_data);
+    unsigned int get_node_index();
+    static void thread_func_process(void *process);
+
+  public:
+    struct TRAJECTORY          trajectory_;
+    plc_modbus::CPLCModbus      plc_;
+    bool            exit_;
+    boost::thread* 	plc_process_thread_;
+    boost::mutex	mutex_trajectorys_;
+};
+
+#endif

include/Planning.h

@@ -0,0 +1,59 @@
+#ifndef __PLANNING__H___
+#define __PLANNING__H___
+
+#include <ros/ros.h>
+#include <nav_msgs/GetPlan.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/tf.h>
+#include "tf2_msgs/TFMessage.h"
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+
+
+#include <string>
+#include <boost/foreach.hpp>
+#include <boost/thread.hpp>
+
+#define forEach BOOST_FOREACH
+
+
+
+class Planning
+{
+public:
+    Planning();
+    ~Planning();
+    bool Init();
+    bool Make_plan(double x0,double y0,double x1,double y1);
+    bool Make_plan(tf::Transform start,tf::Transform target);
+
+protected:
+    
+private:
+    
+    
+    static void tf_callback(const tf2_msgs::TFMessage::ConstPtr& msg,Planning* p);
+    static void thread_publish_path(Planning* plan);
+public:
+    ros::NodeHandle 	nh_;
+    ros::Publisher 	path_publisher_;
+    ros::Publisher 	path_publisher_left_;
+    ros::Publisher 	path_publisher_right_;
+
+    nav_msgs::Path 	path_;
+    nav_msgs::Path 	path_left_;
+    nav_msgs::Path 	path_right_;
+
+    ros::ServiceClient  make_plan_client_;
+    ros::Subscriber 	tf_sub_;
+    boost::thread* 	plan_publisher_thread_;
+    boost::mutex	mutex_path_;
+    tf::Transform    	target_;
+    tf::Transform	    start_pos_;
+    
+    
+};
+
+#endif

include/make_trajectory.h

@@ -0,0 +1,43 @@
+#ifndef __GEN_TRAJECTORY__
+#define __GEN_TRAJECTORY__
+
+#include <tf/tf.h>
+#include "tf2_msgs/TFMessage.h"
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <vector>
+
+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);
+
+
+class MakeTrajectory
+{
+public:
+    MakeTrajectory(tf::Transform t0,tf::Transform t1);
+    bool TransformTrajectory(tf::Transform tf);
+    void SetLRTransform(tf::Transform letf,tf::Transform right);
+    std::vector<tf::Transform> operator[](int n);
+    int Size(){return trajectory_.size();}
+    bool Make();
+    bool IsPoseInTrajectory(tf::Transform pose);
+protected:
+    //计算当前x点的 角度变化率
+    double threta_gradient(double x);
+    tf::Transform  gen_axis_transform_mat();
+protected:
+    std::vector<tf::Transform>      trajectory_;                //雷达轨迹
+    std::vector<tf::Transform>      trajectory_left_;           //左轮轨迹
+    std::vector<tf::Transform>      trajectory_right_;          //右轮轨迹
+
+    tf::Transform                   axis_transform_;
+    tf::Transform                   position0_;
+    tf::Transform                   position1_;
+
+    tf::Transform                   left_tf_;
+    tf::Transform                   right_tf_;
+    Eigen::MatrixXd                 line_param_;                //曲线参数
+};
+
+#endif

src/PLCModbus.cpp

@@ -0,0 +1,163 @@
+#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, true);
+    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)
+{
+    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)
+{
+    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)
+{
+    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)
+{
+    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)
+{
+    int rc = modbus_write_register(this->ctx, addr, src[0]);
+    if (rc != 1)
+    {
+        return false;
+    }
+    else
+    {
+        return true;
+    }
+}
+
+} // namespace plc_modbus

src/PLCProcess.cpp

@@ -0,0 +1,132 @@
+#include "PLCProcess.h"
+
+PLCProcess::PLCProcess()
+{
+    plc_modbus::CPLCModbus plc_ = new plc_modbus::CPLCModbus();
+    plc_process_thread_ = new boost::thread(boost::bind(PLCProcess::thread_func_process, this));
+}
+
+PLCProcess::~PLCProcess()
+{
+    plc_process_thread_->interrupt();
+    plc_process_thread_->join();
+    delete plc_process_thread_;
+}
+
+void PLCProcess::Update(std::vector<std::vector<tf::Transform>> trajectorys)
+{
+    split_trajectorys(trajectorys);
+    trajectory_.bUpdate = true;
+}
+
+bool PLCProcess::split_trajectorys(std::vector<std::vector<tf::Transform>> trajectorys)
+{
+    if (trajectorys.size()>=3)
+    {
+        std::vector<tf::Transform> left = trajectorys[0];
+        std::vector<tf::Transform> right = trajectorys[2];
+        trajectory_.left_trajectorys_.clear();
+        trajectory_.right_trajectorys_.clear();
+        int groups = left.size() / POSE_COUNT + 1;
+        for (int i = 0; i < groups; i++)
+        {
+            trajectory_.left_trajectorys_.push_back(std::vector<tf::Transform>());
+            for (int j = 0; j < std::min((int)POSE_COUNT, (int)(left.size() - (i * POSE_COUNT))); j++)
+            {
+                trajectory_.left_trajectorys_[i].push_back(left[i*POSE_COUNT+j]);
+            }
+        }
+        for (int i = 0; i < groups; i++)
+        {
+            trajectory_.right_trajectorys_.push_back(std::vector<tf::Transform>());
+            for (int j = 0; j < std::min((int)POSE_COUNT, (int)(right.size() - (i * POSE_COUNT))); j++)
+            {
+                trajectory_.right_trajectorys_[i].push_back(right[i * POSE_COUNT + j]);
+            }
+        }
+        return true;
+    }else{
+        return false;
+    }
+}
+
+bool PLCProcess::send_trajectory(std::vector<tf::Transform> trajectory_l, std::vector<tf::Transform> trajectory_r)
+{
+    std::vector<uint16_t> trajectory_combined = std::vector<uint16_t>();
+    std::vector<uint16_t> temp = std::vector<uint16_t>();
+    uint16_t *arr = new uint16_t(sizeof(uint16_t) * POSE_COUNT*2);
+    temp = absolute_convert_to_relative(trajectory_l);
+    trajectory_combined.insert(trajectory_combined.end(),temp.begin(),temp.end());
+    temp = absolute_convert_to_relative(trajectory_r);
+    trajectory_combined.insert(trajectory_combined.end(), temp.begin(), temp.end());
+    if(!trajectory_combined.empty())
+    {
+        memcpy(arr, &trajectory_combined[0], trajectory_combined.size() * sizeof(uint16_t));
+    }
+    return plc_.update_trajectory(true, arr);
+}
+//根据tf绝对路径，获得相对路径vector
+std::vector<uint16_t> absolute_convert_to_relative(std::vector<tf::Transform> tf_data)
+{
+    uint16_t distance = 0;
+    tf::Vector3 pos0, pos1;
+    std::vector<uint16_t> relative_path = std::vector<uint16_t>();
+    for (int i = 0; i < tf_data.size() - 1; i++)
+    {
+        pos0 = tf_data[i].getOrigin();
+        pos1 = tf_data[i+1].getOrigin();
+        distance = (uint16_t)(1000*sqrt(pow(pos1.x() - pos0.x(), 2.0) + pow(pos1.y() - pos0.y(), 2.0) + pow(pos1.z() - pos0.z(), 2.0)));
+        relative_path.push_back(distance);
+    }
+    for (int i = POSE_COUNT;i>relative_path.size();i--)
+    {
+        relative_path.push_back((uint16_t)0);
+    }
+    return relative_path;
+}
+//获取当前节点
+unsigned int PLCProcess::get_node_index()
+{
+    return (unsigned int)plc_.get_current_stage();
+}
+//线程自动判断运动状态
+void PLCProcess::thread_func_process(void *process)
+{
+    PLCProcess *my_process = (PLCProcess *)process;
+    int trajectory_group_index = 0;
+    if (!my_process)
+        return;
+    while (true)
+    {
+        //识别到路径更新指令，初始化轮数值
+        if (my_process->trajectory_.bUpdate)
+        {
+            //my_process->mutex_trajectorys_.lock();
+            trajectory_group_index = 0;
+            //my_process->mutex_trajectorys_.unlock();
+        }
+
+        unsigned int current_node_index = my_process->get_node_index();
+        //接收到更新指令，或路径使用完毕，需要更新
+        if (trajectory_group_index == 0 || current_node_index == 9)
+        {
+            if (my_process->trajectory_.left_trajectorys_.size() > trajectory_group_index)
+            {
+                trajectory_group_index++;
+                my_process->send_trajectory(my_process->trajectory_.left_trajectorys_[trajectory_group_index], my_process->trajectory_.right_trajectorys_[trajectory_group_index]);
+            }
+            else
+            {
+                //无路径或已更新完毕
+                if (current_node_index == 9)
+                {
+                    printf("路径已运动完成");
+                }
+                else
+                {
+                    printf("未找到路径");
+                }
+            }
+        }
+    }
+}

src/Planning.cpp

@@ -0,0 +1,279 @@
+#include "Planning.h"
+#include "make_trajectory.h"
+#include <math.h>
+#include <time.h>
+
+void Planning::tf_callback(const tf2_msgs::TFMessage::ConstPtr& msg,Planning* p)
+{
+    static int n=0;
+    
+    if(msg->transforms[0].header.frame_id=="map" && msg->transforms[0].child_frame_id=="laser")
+    {
+	double x=msg->transforms[0].transform.translation.x;
+    double y=msg->transforms[0].transform.translation.y;
+    double z=msg->transforms[0].transform.translation.z;
+    double rx=msg->transforms[0].transform.rotation.x;
+    double ry=msg->transforms[0].transform.rotation.y;
+    double rz=msg->transforms[0].transform.rotation.z;
+    double rw=msg->transforms[0].transform.rotation.w;
+    clock_t t1,t2;
+    t1=clock();
+    Planning* plan=p;
+    tf::Transform target;
+    tf::Matrix3x3 rotation;
+    rotation.setRPY(0,0,0);
+    target.setBasis(rotation);
+    target.setOrigin(tf::Vector3(0,0,0));
+
+    tf::Quaternion quat(rx,ry,rz,rw);
+    tf::Transform start(quat,tf::Vector3(x,y,z));
+
+    plan->Make_plan(target,start);
+
+    ROS_INFO(" x= %.3f  y=%.3f\n",x,y);
+    t2=clock();
+    ROS_INFO(" make plan time = %.3f\n",t2-t1);
+    }
+}
+
+
+
+Planning::Planning()
+{
+    plan_publisher_thread_=new boost::thread(boost::bind(Planning::thread_publish_path,this));
+    
+}
+
+Planning::~Planning()
+{
+    plan_publisher_thread_->interrupt();
+    plan_publisher_thread_->join();
+    delete plan_publisher_thread_;
+}
+
+void Planning::thread_publish_path(Planning* plan)
+{
+    if(!plan)
+	return ;
+    ros::Rate loop_rate(5);
+    while(ros::ok())
+    {
+	if(plan->path_.poses.size()!=0)
+	{
+	    plan->mutex_path_.lock();
+	    plan->path_publisher_.publish(plan->path_);
+        plan->path_publisher_left_.publish(plan->path_left_);
+        plan->path_publisher_right_.publish(plan->path_right_);
+	    plan->mutex_path_.unlock();
+	
+	    loop_rate.sleep();
+	}
+    }
+}
+
+bool Planning::Init()
+{
+   /* std::string service_name = "move_base/make_plan";
+    //等待服务空闲，如果已经在运行这个服务，会等到运行结束。
+    while (!ros::service::waitForService(service_name, ros::Duration(3.0)))
+    {
+	ROS_INFO("Waiting for service move_base/make_plan to become available");
+    }
+    
+    make_plan_client_ = nh_.serviceClient<nav_msgs::GetPlan>(service_name, true);
+    if (!make_plan_client_) 
+    {
+	ROS_FATAL("Could not initialize get plan service from %s",
+	make_plan_client_.getService().c_str());
+	return false;
+    }*/
+    
+    
+    tf_sub_ = nh_.subscribe<tf2_msgs::TFMessage>("tf", 1,boost::bind(&Planning::tf_callback,_1,this));
+
+    path_publisher_ = nh_.advertise<nav_msgs::Path>("get_plan_center",1, true);
+    path_publisher_left_ = nh_.advertise<nav_msgs::Path>("get_plan_left",1, true);
+    path_publisher_right_ = nh_.advertise<nav_msgs::Path>("get_plan_right",1, true);
+    return true;
+}
+
+bool Planning::Make_plan(double x0,double y0,double x1,double y1)
+{
+    nav_msgs::GetPlan srv;
+    srv.request.start.header.frame_id ="map";
+    srv.request.start.pose.position.x = x0;//初始位置x坐标
+    srv.request.start.pose.position.y = y0;//初始位置y坐标
+    srv.request.start.pose.orientation.w = 1.0;//方向
+    srv.request.goal.header.frame_id = "map";
+    srv.request.goal.pose.position.x = x1;//终点坐标
+    srv.request.goal.pose.position.y = y1;
+    srv.request.goal.pose.orientation.w = 1.0;
+    srv.request.tolerance = 0.5;//如果不能到达目标，最近可到的约束
+    
+    
+    // Perform the actual path planner call
+//执行实际路径规划器
+    if (make_plan_client_.call(srv)) 
+    {
+//srv.response.plan.poses 为保存结果的容器，遍历取出
+	if (!srv.response.plan.poses.empty()) 
+	{
+        
+	    ros::Time current_time, last_time;
+	    current_time = ros::Time::now();
+	    last_time = ros::Time::now();
+
+	    //nav_msgs::Path path;
+	    boost::mutex::scoped_lock lock(mutex_path_);
+	    path_=nav_msgs::Path();
+	    path_.header.stamp=current_time;
+	    path_.header.frame_id="map";
+	   // path_.poses.clear();
+	    int n=-1;
+	    forEach(const geometry_msgs::PoseStamped &p, srv.response.plan.poses) 
+	    {
+		n++;
+		if(n!=0&& n!=srv.response.plan.poses.size()-1)
+		{
+		    
+		    if(n%10!=0)
+		    continue;
+		}
+		double x = p.pose.position.x;
+	        double y = p.pose.position.y;
+	        double th = 0.0;
+	        double vx = 0.1;
+	        double vy = -0.1;
+	        double vth = 0.1;
+		geometry_msgs::PoseStamped this_pose_stamped;
+		this_pose_stamped.pose.position.x = x;
+		this_pose_stamped.pose.position.y = y;
+
+		geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(th);
+		this_pose_stamped.pose.orientation.x = goal_quat.x;
+		this_pose_stamped.pose.orientation.y = goal_quat.y;
+		this_pose_stamped.pose.orientation.z = goal_quat.z;
+		this_pose_stamped.pose.orientation.w = goal_quat.w;
+
+		this_pose_stamped.header.stamp=current_time;
+		this_pose_stamped.header.frame_id="map";
+		path_.poses.push_back(this_pose_stamped);
+		//ROS_INFO("x = %f, y = %f", p.pose.position.x, p.pose.position.y);
+            
+            
+	    }
+	    
+	    printf("update :%d points\n",path_.poses.size());
+	}
+	else {
+	    ROS_WARN("Got empty plan");
+	}
+    }
+    else {
+    ROS_ERROR("Failed to call service %s - is the robot moving?",
+    make_plan_client_.getService().c_str());
+    }
+    
+    return true;
+}
+
+bool Planning::Make_plan(tf::Transform start,tf::Transform target)
+{
+    MakeTrajectory trajectory(start,target);
+    tf::Transform left=tf::Transform(tf::Quaternion(0,0,0),tf::Vector3(0.0,0.1,0));
+    tf::Transform right=tf::Transform(tf::Quaternion(0,0,0),tf::Vector3(0.0,-0.1,0));
+    trajectory.SetLRTransform(left,right);
+    trajectory.Make();
+    
+    if(trajectory.Size()>0)
+    {
+        ros::Time current_time, last_time;
+	    current_time = ros::Time::now();
+	    last_time = ros::Time::now();
+
+        boost::mutex::scoped_lock lock(mutex_path_);
+	    path_=nav_msgs::Path();
+	    path_.header.stamp=current_time;
+	    path_.header.frame_id="map";
+
+        path_left_=nav_msgs::Path();
+	    path_left_.header.stamp=current_time;
+	    path_left_.header.frame_id="map";
+
+        path_right_=nav_msgs::Path();
+	    path_right_.header.stamp=current_time;
+	    path_right_.header.frame_id="map";
+
+	    // path_.poses.clear();
+	    for(int i=0;i<trajectory.Size();++i) 
+	    {
+            tf::Transform pose=trajectory[i][0];
+	        double x = pose.getOrigin().getX();
+            double y = pose.getOrigin().getY();
+            double th = 0.0;
+            double vx = 0.1;
+            double vy = -0.1;
+            double vth = 0.1;
+
+		    geometry_msgs::PoseStamped this_pose_stamped;
+		    this_pose_stamped.pose.position.x = x;
+		    this_pose_stamped.pose.position.y = y;
+
+	    	geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(th);
+	    	this_pose_stamped.pose.orientation.x = goal_quat.x;
+	    	this_pose_stamped.pose.orientation.y = goal_quat.y;
+	    	this_pose_stamped.pose.orientation.z = goal_quat.z;
+	    	this_pose_stamped.pose.orientation.w = goal_quat.w;
+
+	    	this_pose_stamped.header.stamp=current_time;
+	    	this_pose_stamped.header.frame_id="map";
+	    	path_left_.poses.push_back(this_pose_stamped);
+
+
+            pose=trajectory[i][1];
+	        x = pose.getOrigin().getX();
+            y = pose.getOrigin().getY();
+
+		    this_pose_stamped.pose.position.x = x;
+		    this_pose_stamped.pose.position.y = y;
+
+	    	this_pose_stamped.pose.orientation.x = goal_quat.x;
+	    	this_pose_stamped.pose.orientation.y = goal_quat.y;
+	    	this_pose_stamped.pose.orientation.z = goal_quat.z;
+	    	this_pose_stamped.pose.orientation.w = goal_quat.w;
+
+	    	this_pose_stamped.header.stamp=current_time;
+	    	this_pose_stamped.header.frame_id="map";
+	    	path_.poses.push_back(this_pose_stamped);
+
+            pose=trajectory[i][2];
+	        x = pose.getOrigin().getX();
+            y = pose.getOrigin().getY();
+
+		    this_pose_stamped.pose.position.x = x;
+		    this_pose_stamped.pose.position.y = y;
+
+	    	this_pose_stamped.pose.orientation.x = goal_quat.x;
+	    	this_pose_stamped.pose.orientation.y = goal_quat.y;
+	    	this_pose_stamped.pose.orientation.z = goal_quat.z;
+	    	this_pose_stamped.pose.orientation.w = goal_quat.w;
+
+	    	this_pose_stamped.header.stamp=current_time;
+	    	this_pose_stamped.header.frame_id="map";
+	    	path_right_.poses.push_back(this_pose_stamped);
+
+	    }    
+	    printf("update :%d points\n",path_.poses.size());
+    }
+	else {
+	    ROS_WARN("Got empty plan");
+	}
+    return true;
+}
+
+
+
+
+
+
+

src/plc_test.cpp

@@ -0,0 +1,56 @@
+#include "PLCModbus.h"
+
+
+int main()
+{
+    plc_modbus::CPLCModbus cp = new plc_modbus::CPLCModbus();
+    std::cout<< "handle==NULL?:" << cp.ctx <<std::endl;
+    //bool result = cp.connect("192.168.2.126", 1502);
+    bool result = cp.connect("192.168.0.30", 30001);
+    if (result)
+    {
+        int count=0;
+        // while(count<1000001){
+        //     if(count>1000000){
+        //         count=0;
+        //     }else
+        //     {
+        //         count++;
+        //     }
+            
+            uint16_t *write_data = (uint16_t *)malloc(60 * sizeof(uint16_t));
+            uint16_t *read_data = (uint16_t *)malloc(60 * sizeof(uint16_t));
+            memset(write_data, 4, 60 * sizeof(uint16_t));
+            memset(read_data, 0, 60 * sizeof(uint16_t));
+            bool write_result = cp.write_registers((uint16_t)0, 1, write_data);
+            //std::cout << cp.get_current_stage() << std::endl;
+            //std::cout << cp.get_finish_status() << std::endl;
+            //bool write_result = cp.update_trajectory((uint16_t)555, write_data);
+            bool read_result = cp.read_registers((uint16_t)0, 40, read_data);
+            //std::cout << write_result << "," << read_result << std::endl;
+            if (read_result)
+            {
+                std::cout << "************* read result ************" << std::endl;
+                for (size_t i = 0; i < 60; i++)
+                {
+                    std::cout << read_data[i];
+                    if (i == 59)
+                    {
+                        std::cout << std::endl;
+                    }
+                    else
+                    {
+                        if(i%10==0)
+                            std::cout << ",\n";
+                        else
+                        {
+                            std::cout << " ";
+                        }
+                        
+                    }
+                }
+            }
+        // }
+    }
+    cp.disconnect();
+}

src/test.cpp

@@ -0,0 +1,9 @@
+#include "PLCModbus.h"
+
+int main()
+{
+    std::cout << "output" << std::endl;
+    plc_modbus::CPLCModbus cp = new plc_modbus::CPLCModbus(0);
+    cp.connect("192.168.2.157",1000);
+    return 0;
+}