linux下如何驱动epos
参见epos官方文档EPOS-Command-Library-En 文档154页详解
1.首先下载linux epos.包 EPOS-Linux-Library-En.zip
2.复制libEposCmd.so.6.2.1.0
sudo cp libEposCmd.so.6.2.1.0
3.创建库
4.创建一个软链接
5.头文件
Copy the file “Defintions.h” to the directory “/usr/include”.
6.Compile the project using on of the following methods:
A make
Switch to the directory “HelloEposCmd” and execute the make* command without
parameters:
$ make
B g++
Switch to the directory “HelloEposCmd” and use the g++* command with parameters:
$ g++ HelloEposCmd.cpp -l EposCmd -o HelloEposCmd
C Eclipse
1) Find the Eclipse project «HelloEposCmd» in the “examples” directory.
2) Import the project into your workspace.
3) Optional: Include the EposCmd library for C++ linker (-L option):EposCmd
7.插入USB操作
在ROS下如何运行
link_directories(/home/hao/catkin_lh/src/movecar/include/movecar) include_directories(/home/hao/catkin_lh/src/movecar/include/movecar) add_executable(log_teleop src/log_teleop.cpp) target_link_libraries(log_teleop ${catkin_LIBRARIES} EposCmd) 接受手柄数据,控制小车
讯享网
讯享网/ 订阅了/cmd_vel主题,只要向该主题发布消息,就能实现对控制小车的移动 再正确配置串口就可以了 */ #include<ros/ros.h> #include <sensor_msgs/Joy.h> #include<iostream> #include "std_msgs/String.h" //导航接收 #include <cstdio> #include <iostream> #include "Definitions.h" #include <string.h> #include <sstream> #include <unistd.h> #include <getopt.h> #include <stdlib.h> #include <stdio.h> #include <list> #include <math.h> #include <sys/types.h> #include <unistd.h> #include <stdio.h> #include <sys/times.h> #include <sys/time.h> typedef void* HANDLE; typedef int BOOL; using namespace std; void* g_pKeyHandle = 0; unsigned short g_usNodeId = 1; string g_deviceName; string g_protocolStackName; string g_interfaceName; string g_portName; int g_baudrate = 0; const string g_programName = "EPOS_MAIN"; void* g_pKeyHandle2 = 0; unsigned short g_usNodeId2 = 1; string g_deviceName2; string g_protocolStackName2; string g_interfaceName2; string g_portName2; int g_baudrate2 = 0; const string g_programName2 = "EPOS_MAIN"; #ifndef MMC_SUCCESS #define MMC_SUCCESS 0 #endif #ifndef MMC_FAILED #define MMC_FAILED 1 #endif #ifndef MMC_MAX_LOG_MSG_SIZE #define MMC_MAX_LOG_MSG_SIZE 512 #endif //--------------------------------手柄 void callback(const sensor_msgs::Joy::ConstPtr& Joy); float axis_ang1,axis_lin1,axis_ang2,axis_lin2,axis_ang3,axis_lin3,ltadd1,rtadd1; int ton[11];//axes[]的键 button[]/home/yanlei/桌面 void LogError(string functionName, int p_lResult, unsigned int p_ulErrorCode); void LogInfo(string message); void PrintUsage(); void PrintHeader(); void PrintSettings(); int OpenDevice(unsigned int* p_pErrorCode); int CloseDevice(unsigned int* p_pErrorCode); void SetDefaultParameters(); int ParseArguments(int argc, char argv); int DemoProfilePositionMode(HANDLE p_DeviceHandle, unsigned short p_usNodeId, unsigned int & p_rlErrorCode); int Demo(unsigned int* p_pErrorCode); int VCS_GetVelocityIs(HANDLE,pDeviceName,long* pVelocityIs, unsigned int & p_rlErrorCode) void PrintUsage() { cout << "Usage: HelloEposCmd -h -n 1 -d deviceName -s protocolStackName -i interfaceName -p portName -b baudrate" << endl; cout << "Usage: HelloEposCmd -h -n 1 -d deviceName -s protocolStackName -i interfaceName -p portName -b baudrate" << endl; } void LogError(string functionName, int p_lResult, unsigned int p_ulErrorCode) { cerr << g_programName << ": " << functionName << " failed (result=" << p_lResult << ", errorCode=0x" << std::hex << p_ulErrorCode << ")"<< endl; cerr << g_programName2 << ": " << functionName << " failed (result=" << p_lResult << ", errorCode=0x" << std::hex << p_ulErrorCode << ")"<< endl; } void LogInfo(string message) { cout << message << endl; } void SeparatorLine() { const int lineLength = 60; for(int i=0; i<lineLength; i++) { cout << "-"; } cout << endl; } void PrintSettings() { stringstream msg; msg << "default settings:" << endl; msg << "node id = " << g_usNodeId << endl; msg << "device name = '" << g_deviceName << "'" << endl; msg << "protocal stack name = '" << g_protocolStackName << "'" << endl; msg << "interface name = '" << g_interfaceName << "'" << endl; msg << "port name = '" << g_portName << "'"<< endl; msg << "baudrate = " << g_baudrate; LogInfo(msg.str()); SeparatorLine(); // ------------------------------------------------------- stringstream msg2; msg2 << "default settings:" << endl; msg2 << "node id = " << g_usNodeId2 << endl; msg2 << "device name = '" << g_deviceName2 << "'" << endl; msg2 << "protocal stack name = '" << g_protocolStackName2 << "'" << endl; msg2 << "interface name = '" << g_interfaceName2 << "'" << endl; msg2 << "port name = '" << g_portName2 << "'"<< endl; msg2 << "baudrate = " << g_baudrate2; LogInfo(msg2.str()); SeparatorLine(); } void SetDefaultParameters() { //USB g_usNodeId =1; g_deviceName = "EPOS"; //EPOS version g_protocolStackName = "MAXON_RS232"; //MAXON_RS232 MAXON SERIAL V2 g_interfaceName = "RS232"; //RS232 USB g_portName = "/dev/ttyS100"; // /dev/ttyS1 USB0 g_baudrate = 38400; // //---------------------------------------------------- g_usNodeId2 =2; g_deviceName2 = "EPOS"; //EPOS version g_protocolStackName2 = "MAXON_RS232"; //MAXON_RS232 MAXON SERIAL V2 g_interfaceName2 = "RS232"; //RS232 USB g_portName2 = "/dev/ttyS101"; // /dev/ttyS1 USB0 g_baudrate2 = 38400; // } int OpenDevice(unsigned int* p_pErrorCode) { int lResult = MMC_FAILED; char* pDeviceName = new char[255]; char* pProtocolStackName = new char[255]; char* pInterfaceName = new char[255]; char* pPortName = new char[255]; strcpy(pDeviceName, g_deviceName.c_str()); strcpy(pProtocolStackName, g_protocolStackName.c_str()); strcpy(pInterfaceName, g_interfaceName.c_str()); strcpy(pPortName, g_portName.c_str()); LogInfo("Open device..."); g_pKeyHandle = VCS_OpenDevice(pDeviceName, pProtocolStackName, pInterfaceName, pPortName, p_pErrorCode); if(g_pKeyHandle!=0 && *p_pErrorCode == 0) { unsigned int lBaudrate = 0; unsigned int lTimeout = 0; if(VCS_GetProtocolStackSettings(g_pKeyHandle, &lBaudrate, &lTimeout, p_pErrorCode)!=0) { if(VCS_SetProtocolStackSettings(g_pKeyHandle, g_baudrate, lTimeout, p_pErrorCode)!=0) { if(VCS_GetProtocolStackSettings(g_pKeyHandle, &lBaudrate, &lTimeout, p_pErrorCode)!=0) { if(g_baudrate==(int)lBaudrate) { lResult = MMC_SUCCESS; } } } } } else { g_pKeyHandle = 0; } delete []pDeviceName; delete []pProtocolStackName; delete []pInterfaceName; delete []pPortName; //------------------------------------------------------------------------------------------------ char* pDeviceName2 = new char[255]; char* pProtocolStackName2 = new char[255]; char* pInterfaceName2 = new char[255]; char* pPortName2 = new char[255]; strcpy(pDeviceName2, g_deviceName2.c_str()); strcpy(pProtocolStackName2, g_protocolStackName2.c_str()); strcpy(pInterfaceName2, g_interfaceName2.c_str()); strcpy(pPortName2, g_portName2.c_str()); LogInfo("Open device..."); g_pKeyHandle2 = VCS_OpenDevice(pDeviceName2, pProtocolStackName2, pInterfaceName2, pPortName2, p_pErrorCode); if(g_pKeyHandle2!=0 && *p_pErrorCode == 0) { unsigned int lBaudrate = 0; unsigned int lTimeout = 0; if(VCS_GetProtocolStackSettings(g_pKeyHandle2, &lBaudrate, &lTimeout, p_pErrorCode)!=0) { if(VCS_SetProtocolStackSettings(g_pKeyHandle2, g_baudrate2, lTimeout, p_pErrorCode)!=0) { if(VCS_GetProtocolStackSettings(g_pKeyHandle2, &lBaudrate, &lTimeout, p_pErrorCode)!=0) { if(g_baudrate2==(int)lBaudrate) { lResult = MMC_SUCCESS; } } } } } else { g_pKeyHandle2 = 0; } delete []pDeviceName2; delete []pProtocolStackName2; delete []pInterfaceName2; delete []pPortName2; return lResult; } int CloseDevice(unsigned int* p_pErrorCode) { int lResult = MMC_FAILED; *p_pErrorCode = 0; LogInfo("Close device"); if(VCS_CloseDevice(g_pKeyHandle, p_pErrorCode)!=0 && *p_pErrorCode == 0) { lResult = MMC_SUCCESS; } //-------------------------------------------------------- // int lResult = MMC_FAILED; *p_pErrorCode = 0; LogInfo("Close device"); if(VCS_CloseDevice(g_pKeyHandle2, p_pErrorCode)!=0 && *p_pErrorCode == 0) { lResult = MMC_SUCCESS; } return lResult; } int ParseArguments(int argc, char argv) { int lOption; int lResult = MMC_SUCCESS; // Shut GetOpt error messages down (return '?'): opterr = 0; // Retrieve the options: while ( (lOption = getopt(argc, argv, ":hd:s:i:p:b:n:")) != -1 ) { switch ( lOption ) { case 'h': PrintUsage(); //Usage: HelloEposCmd -h -n 1 -d deviceName //-s protocolStackName -i interfaceName -p portName -b baudrate lResult = 1; break; case 'd': g_deviceName = optarg; //this is a special break; case 's': g_protocolStackName = optarg; break; case 'i': g_interfaceName = optarg; break; case 'p': g_portName = optarg; break; case 'b': g_baudrate = atoi(optarg); break; case 'n': g_usNodeId = (unsigned short)atoi(optarg); break; case '?': // unknown option... stringstream msg; msg << "Unknown option: '" << char(optopt) << "'!"; LogInfo(msg.str()); PrintUsage(); lResult = MMC_FAILED; break; } } //----------------------------------------------------------------- // int lOption; lResult = MMC_SUCCESS; // Shut GetOpt error messages down (return '?'): opterr = 0; // Retrieve the options: while ( (lOption = getopt(argc, argv, ":hd:s:i:p:b:n:")) != -1 ) { switch ( lOption ) { case 'h': PrintUsage(); //Usage: HelloEposCmd -h -n 1 -d deviceName //-s protocolStackName -i interfaceName -p portName -b baudrate lResult = 1; break; case 'd': g_deviceName2 = optarg; //this is a special break; case 's': g_protocolStackName2 = optarg; break; case 'i': g_interfaceName2 = optarg; break; case 'p': g_portName2 = optarg; break; case 'b': g_baudrate2 = atoi(optarg); break; case 'n': g_usNodeId2 = (unsigned short)atoi(optarg); break; case '?': // unknown option... stringstream msg; msg << "Unknown option: '" << char(optopt) << "'!"; LogInfo(msg.str()); PrintUsage(); lResult = MMC_FAILED; break; } } return lResult; } int DemoProfilePositionMode(HANDLE p_DeviceHandle, unsigned short p_usNodeId, unsigned int & p_rlErrorCode) { int lResult = MMC_SUCCESS; stringstream msg; msg << "set profile position mode, node = " << p_usNodeId; LogInfo(msg.str()); if(VCS_ActivateProfilePositionMode(p_DeviceHandle, p_usNodeId, &p_rlErrorCode) == 0) { LogError("VCS_ActivateProfilePositionMode", lResult, p_rlErrorCode); lResult = MMC_FAILED; } else { list<long> positionList; positionList.push_back(5000); positionList.push_back(-20000); positionList.push_back(20000); positionList.push_back(-10000); positionList.push_back(10000); positionList.push_back(-10000); positionList.push_back(5000); for(list<long>::iterator it = positionList.begin(); it !=positionList.end(); it++) { long targetPosition = (*it); stringstream msg; msg << "move to position = " << targetPosition << ", node = " << p_usNodeId; LogInfo(msg.str()); if(VCS_MoveToPosition(p_DeviceHandle, p_usNodeId, targetPosition, 0, 1, &p_rlErrorCode) == 0) { LogError("VCS_MoveToPosition", lResult, p_rlErrorCode); lResult = MMC_FAILED; break; } sleep(1); } if(lResult == MMC_SUCCESS) { LogInfo("halt position movement"); if(VCS_HaltPositionMovement(p_DeviceHandle, p_usNodeId, &p_rlErrorCode) == 0) { LogError("VCS_HaltPositionMovement", lResult, p_rlErrorCode); lResult = MMC_FAILED; } } } return lResult; } bool DemoProfileVelocityMode(HANDLE p_DeviceHandle, unsigned short p_usNodeId, unsigned int & p_rlErrorCode) { int lResult = MMC_SUCCESS; stringstream msg; msg << "set profile velocity mode, node = " << p_usNodeId; LogInfo(msg.str()); if(VCS_ActivateProfileVelocityMode(p_DeviceHandle, p_usNodeId, &p_rlErrorCode) == 0) { LogError("VCS_ActivateProfileVelocityMode", lResult, p_rlErrorCode); lResult = MMC_FAILED; } else { list<long> velocityList; velocityList.push_back(500); velocityList.push_back(300); velocityList.push_back(600); velocityList.push_back(900); velocityList.push_back(200); for(list<long>::iterator it = velocityList.begin(); it !=velocityList.end(); it++) { long targetvelocity = (*it); //mubiaosudu stringstream msg; msg << "move with target velocity = " << targetvelocity << " rpm, node = " << p_usNodeId; LogInfo(msg.str()); if(VCS_MoveWithVelocity(p_DeviceHandle, p_usNodeId, targetvelocity, &p_rlErrorCode) == 0) { lResult = MMC_FAILED; LogError("VCS_MoveWithVelocity", lResult, p_rlErrorCode); break; } sleep(3); } if(lResult == MMC_SUCCESS) { LogInfo("halt velocity movement"); if(VCS_HaltVelocityMovement(p_DeviceHandle, p_usNodeId, &p_rlErrorCode) == 0) { lResult = MMC_FAILED; LogError("VCS_HaltVelocityMovement", lResult, p_rlErrorCode); } } } return lResult; } int PrepareDemo(unsigned int* p_pErrorCode) { int lResult = MMC_SUCCESS; BOOL oIsFault = 0; if(VCS_GetFaultState(g_pKeyHandle, g_usNodeId, &oIsFault, p_pErrorCode ) == 0) { LogError("VCS_GetFaultState", lResult, *p_pErrorCode); lResult = MMC_FAILED; } if(lResult==0) { if(oIsFault) { stringstream msg; msg << "clear fault, node = '" << g_usNodeId << "'"; LogInfo(msg.str()); if(VCS_ClearFault(g_pKeyHandle, g_usNodeId, p_pErrorCode) == 0) { LogError("VCS_ClearFault", lResult, *p_pErrorCode); lResult = MMC_FAILED; } } if(lResult==0) { BOOL oIsEnabled = 0; if(VCS_GetEnableState(g_pKeyHandle, g_usNodeId, &oIsEnabled, p_pErrorCode) == 0) { LogError("VCS_GetEnableState", lResult, *p_pErrorCode); lResult = MMC_FAILED; } if(lResult==0) { if(!oIsEnabled) { if(VCS_SetEnableState(g_pKeyHandle, g_usNodeId, p_pErrorCode) == 0) { LogError("VCS_SetEnableState", lResult, *p_pErrorCode); lResult = MMC_FAILED; } } } } } //----------------------------------------------------------------------------------------- lResult = MMC_SUCCESS; oIsFault = 0; if(VCS_GetFaultState(g_pKeyHandle2, g_usNodeId2, &oIsFault, p_pErrorCode ) == 0) { LogError("VCS_GetFaultState", lResult, *p_pErrorCode); lResult = MMC_FAILED; } if(lResult==0) { if(oIsFault) { stringstream msg; msg << "clear fault, node = '" << g_usNodeId2 << "'"; LogInfo(msg.str()); if(VCS_ClearFault(g_pKeyHandle2, g_usNodeId2, p_pErrorCode) == 0) { LogError("VCS_ClearFault", lResult, *p_pErrorCode); lResult = MMC_FAILED; } } if(lResult==0) { BOOL oIsEnabled = 0; if(VCS_GetEnableState(g_pKeyHandle2, g_usNodeId2, &oIsEnabled, p_pErrorCode) == 0) { LogError("VCS_GetEnableState", lResult, *p_pErrorCode); lResult = MMC_FAILED; } if(lResult==0) { if(!oIsEnabled) { if(VCS_SetEnableState(g_pKeyHandle2, g_usNodeId2, p_pErrorCode) == 0) { LogError("VCS_SetEnableState", lResult, *p_pErrorCode); lResult = MMC_FAILED; } } } } } return lResult; } int Demo(unsigned int* p_pErrorCode) { int lResult = MMC_SUCCESS; unsigned int lErrorCode = 0; lResult = DemoProfileVelocityMode(g_pKeyHandle, g_usNodeId, lErrorCode); //--------------- if(lResult != MMC_SUCCESS) { LogError("DemoProfileVelocityMode", lResult, lErrorCode); } else { // lResult = DemoProfilePositionMode(g_pKeyHandle, g_usNodeId, lErrorCode); //这里去掉了 // if(lResult != MMC_SUCCESS) // { // LogError("DemoProfilePositionMode", lResult, lErrorCode); // } // else // { if(VCS_SetDisableState(g_pKeyHandle, g_usNodeId, &lErrorCode) == 0) //-------------------- { LogError("VCS_SetDisableState", lResult, lErrorCode); lResult = MMC_FAILED; } // } } return lResult; } void PrintHeader() //can clear that..------------------------------------------------------ { // Epos Command Library Example Program, (c) maxonmotor ag 2014-2017 SeparatorLine(); //--------------------------------------------------------------- LogInfo("Epos Command Library Example Program, (c) maxonmotor ag 2014-2017"); SeparatorLine(); } /*/ float linear_temp=0,angular_temp=0;//暂存的线速度和角速度 float lt_temp=0,rt_temp=0; int lendmotor=0,rendmotor=0; // void callback(const sensor_msgs::Joy::ConstPtr& Joy) //订阅方把订阅的话题中的消息发布出来 { //-----------------罗技手柄的接受程序------目的在于重新编码,便于编程--------------------------------------------------- axis_lin1= Joy->axes[1]; //左边遥感 axis_ang1= Joy->axes[0]; axis_lin2= Joy->axes[4]; //右边摇杆 axis_ang2= Joy->axes[3]; axis_lin3= Joy->axes[7]; //八方向控制 axis_ang3= Joy->axes[6]; ltadd1= Joy->axes[2]; rtadd1= Joy->axes[5]; for(int i=0;i<9;i++) { ton[i]=Joy->buttons[i]; //依次A B X Y 0123 LB RB BACK START 4567 LOG 8 } ROS_INFO("linear1:%f angular1:%f linear2:%f angular2:%f linear3:%f angular3:%f ltadd1:%f rtadd1:%f", axis_lin1,axis_ang1,axis_lin2,axis_ang2,axis_lin3,axis_ang3,ltadd1,rtadd1); ROS_INFO("bt0:%d bt1:%d bt2:%d bt3:%d bt4:%d bt5:%d bt6:%d bt7:%d bt8:%d", ton[0],ton[1],ton[2],ton[3],ton[4],ton[5],ton[6],ton[7],ton[8]); //----------------------------------------------------------------------------------------------- unsigned int ulErrorCode = 0; int lResult = MMC_SUCCESS; unsigned int lErrorCode = 0; if(ton[7]) //start 开启设备 { OpenDevice(&ulErrorCode); PrepareDemo(&ulErrorCode); LogInfo("ActivateProfileVelocityMode node1,yes"); VCS_ActivateProfileVelocityMode(g_pKeyHandle, g_usNodeId, &lErrorCode); //激活速度模式 LogInfo("ActivateProfileVelocityMode node2,yes"); VCS_ActivateProfileVelocityMode(g_pKeyHandle2, g_usNodeId2, &lErrorCode); } linear_temp = axis_lin1 ;//获取/cmd_vel的线速度.m/s angular_temp = axis_ang1 ;//获取/cmd_vel的角速度,rad/s 这里将罗技手柄的摇动杆子1写了进去 linear_temp=linear_temp*750; // 将0.5的浮点书扩大十倍 angular_temp =angular_temp*450; lt_temp = ltadd1 ; // rt_temp = rtadd1 ; // lt_temp=(1-lt_temp)*200; // 左边减速 右边加速 rt_temp =(1-rt_temp)*200; // ROS_INFO("I heard: [%f]",linear_temp); // ROS_INFO("I heard: [%f]",angular_temp); int int_linear_temp = (int)linear_temp; int int_angular_temp = (int)angular_temp; //速度的int转化 int int_rt_temp = (int)rt_temp; int int_lt_temp = (int)lt_temp; // ROS_INFO("I heard: [%d]",int_linear_temp); // ROS_INFO("I heard: [%d]",int_angular_temp); if(ton[6]) //back 就可以关闭电机驱动器 { LogInfo("halt velocity movement,yes node1"); VCS_HaltVelocityMovement(g_pKeyHandle, g_usNodeId, &lErrorCode); //禁止速度模式 VCS_SetDisableState(g_pKeyHandle, g_usNodeId, &lErrorCode); //释放标志位 //------------------------------------------------------------------------------------------------- LogInfo("halt velocity movement,yes node2"); VCS_HaltVelocityMovement(g_pKeyHandle2, g_usNodeId2, &lErrorCode); //禁止速度模式 VCS_SetDisableState(g_pKeyHandle2, g_usNodeId2, &lErrorCode); //释放标志位 CloseDevice(&ulErrorCode); ROS_INFO("CloseDevice,yes node1"); ROS_INFO("CloseDevice,yes node2"); } else if(int_linear_temp>0) { lendmotor=int_linear_temp*(-1)+int_angular_temp-int_rt_temp+int_lt_temp; rendmotor=int_linear_temp+int_angular_temp+int_rt_temp-int_lt_temp; } else if(int_linear_temp<0) { lendmotor=int_linear_temp*(-1)-int_angular_temp+int_rt_temp-int_lt_temp; rendmotor=int_linear_temp-int_angular_temp-int_rt_temp+int_lt_temp; } else { int tmprate=0; if(ton[5]) {tmprate=400;} if(ton[0]) {lendmotor=-100-tmprate;rendmotor=100+tmprate;} else if(ton[1]) {lendmotor=-200-tmprate;rendmotor=200+tmprate;} else if(ton[2]) {lendmotor=-300-tmprate;rendmotor=300+tmprate;} else if(ton[3]) {lendmotor=-400-tmprate;rendmotor=400+tmprate;} else {lendmotor=0; rendmotor=0;} // lendmotor=0; // rendmotor=0; } LogInfo("set velocity ,yes"); VCS_MoveWithVelocity(g_pKeyHandle, g_usNodeId, lendmotor, &lErrorCode); VCS_MoveWithVelocity(g_pKeyHandle2, g_usNodeId2, rendmotor, &lErrorCode); ROS_INFO("this temp v: [%d] [%d]",lendmotor,rendmotor); VCS_GetVelocityIs(g_pKeyHandle, g_usNodeId,*pVelocityIs, &lErrorCode) ROS_INFO("auctual velocity: [%ld]",*pVelocityIs); } //---------------------------回调函数接收---------------------------------------- void messageCallback(const std_msgs::String::ConstPtr& msg4) { //ROS_INFO("I heard: [%s]", msg4->data.c_str()); } int main(int argc, char argv) { //-------------------电机初始化------------------------------- unsigned int lErrorCode = 0; int lRWORDesult = MMC_FAILED; unsigned int ulErrorCode = 0; PrintHeader(); SetDefaultParameters(); if((lResult = ParseArguments(argc, argv))!=MMC_SUCCESS){return lResult;} PrintSettings(); //output *efault settings: example protocal stack name = 'MAXON_RS232 //----------logstick-------------------------------------------------------- ros::init(argc, argv, "log_teleop"); ros::NodeHandle n; //实例化节点 ros::Subscriber sub ; sub = n.subscribe<sensor_msgs::Joy>("joy",10,callback); //订阅游戏手柄发来的数据 // ------------NAV100---------------------------------------------------- ros::Subscriber sub1 = n.subscribe("message", 1000, messageCallback); // 这里的sub1是什么意思 ros::spin(); return 0; }
QT下运行的epos教程(自行揣摩)
EPOS CMakeLists 文件的写法 (需要注意的链接.so文件和链接头文件)
后者是epos
cmake_minimum_required(VERSION 2.8) project(EPOS_QT) link_directories(/home/hao/EPOS_Linux_Library/include) add_executable(${PROJECT_NAME} "main.cpp") target_link_libraries(${PROJECT_NAME} libEposCmd.so)
讯享网//============================================================================ // Name : main.cpp // Author : Dawid Sienkiewicz // Version : // Copyright : maxon motor ag 2014 // Description : Hello Epos in C++, Ansi-style //============================================================================ #include <iostream> #include "Definitions.h" #include <string.h> #include <sstream> #include <unistd.h> #include <getopt.h> #include <stdlib.h> #include <stdio.h> #include <list> #include <math.h> #include <sys/types.h> #include <unistd.h> #include <stdio.h> #include <sys/times.h> #include <sys/time.h> typedef void* HANDLE; typedef int BOOL; using namespace std; void* g_pKeyHandle2 = 0; unsigned short g_usNodeId2 = 1; string g_deviceName2; string g_protocolStackName2; string g_interfaceName2; string g_portName2; int g_baudrate2 = 0; const string g_programName2 = "EPOS_MAIN"; #ifndef MMC_SUCCESS #define MMC_SUCCESS 0 #endif #ifndef MMC_FAILED #define MMC_FAILED 1 #endif #ifndef MMC_MAX_LOG_MSG_SIZE #define MMC_MAX_LOG_MSG_SIZE 512 #endif void LogError(string functionName, int p_lResult, unsigned int p_ulErrorCode); void LogInfo(string message); void PrintUsage(); void PrintHeader(); void PrintSettings(); int OpenDevice(unsigned int* p_pErrorCode); int CloseDevice(unsigned int* p_pErrorCode); void SetDefaultParameters(); int ParseArguments(int argc, char argv); int DemoProfilePositionMode(HANDLE p_DeviceHandle, unsigned short p_usNodeId, unsigned int & p_rlErrorCode); int Demo(unsigned int* p_pErrorCode); void PrintUsage() { cout << "Usage: HelloEposCmd -h -n 1 -d deviceName -s protocolStackName -i interfaceName -p portName -b baudrate" << endl; } void LogError(string functionName, int p_lResult, unsigned int p_ulErrorCode) { cerr << g_programName2 << ": " << functionName << " failed (result=" << p_lResult << ", errorCode=0x" << std::hex << p_ulErrorCode << ")"<< endl; } void LogInfo(string message) { cout << message << endl; } void SeparatorLine() { const int lineLength = 60; for(int i=0; i<lineLength; i++) { cout << "-"; } cout << endl; } void PrintSettings() { stringstream msg; msg << "default settings:" << endl; msg << "node id = " << g_usNodeId2 << endl; msg << "device name = '" << g_deviceName2 << "'" << endl; msg << "protocal stack name = '" << g_protocolStackName2 << "'" << endl; msg << "interface name = '" << g_interfaceName2 << "'" << endl; msg << "port name = '" << g_portName2 << "'"<< endl; msg << "baudrate = " << g_baudrate2; LogInfo(msg.str()); SeparatorLine(); } void SetDefaultParameters() { //USB g_usNodeId2 =2; g_deviceName2 = "EPOS"; //EPOS version g_protocolStackName2 = "MAXON_RS232"; //MAXON_RS232 MAXON SERIAL V2 g_interfaceName2 = "RS232"; //RS232 USB g_portName2 = "/dev/ttyS101"; // /dev/ttyS1 USB0 g_baudrate2 = 38400; // } int OpenDevice(unsigned int* p_pErrorCode) { int lResult = MMC_FAILED; char* pDeviceName2 = new char[255]; char* pProtocolStackName2 = new char[255]; char* pInterfaceName2 = new char[255]; char* pPortName2 = new char[255]; strcpy(pDeviceName2, g_deviceName2.c_str()); strcpy(pProtocolStackName2, g_protocolStackName2.c_str()); strcpy(pInterfaceName2, g_interfaceName2.c_str()); strcpy(pPortName2, g_portName2.c_str()); LogInfo("Open device..."); g_pKeyHandle2 = VCS_OpenDevice(pDeviceName2, pProtocolStackName2, pInterfaceName2, pPortName2, p_pErrorCode); if(g_pKeyHandle2!=0 && *p_pErrorCode == 0) { unsigned int lBaudrate = 0; unsigned int lTimeout = 0; if(VCS_GetProtocolStackSettings(g_pKeyHandle2, &lBaudrate, &lTimeout, p_pErrorCode)!=0) { if(VCS_SetProtocolStackSettings(g_pKeyHandle2, g_baudrate2, lTimeout, p_pErrorCode)!=0) { if(VCS_GetProtocolStackSettings(g_pKeyHandle2, &lBaudrate, &lTimeout, p_pErrorCode)!=0) { if(g_baudrate2==(int)lBaudrate) { lResult = MMC_SUCCESS; } } } } } else { g_pKeyHandle2 = 0; } delete []pDeviceName2; delete []pProtocolStackName2; delete []pInterfaceName2; delete []pPortName2; return lResult; } int CloseDevice(unsigned int* p_pErrorCode) { int lResult = MMC_FAILED; *p_pErrorCode = 0; LogInfo("Close device"); if(VCS_CloseDevice(g_pKeyHandle2, p_pErrorCode)!=0 && *p_pErrorCode == 0) { lResult = MMC_SUCCESS; } return lResult; } int ParseArguments(int argc, char argv) { int lOption; int lResult = MMC_SUCCESS; // Shut GetOpt error messages down (return '?'): opterr = 0; // Retrieve the options: while ( (lOption = getopt(argc, argv, ":hd:s:i:p:b:n:")) != -1 ) { switch ( lOption ) { case 'h': PrintUsage(); //Usage: HelloEposCmd -h -n 1 -d deviceName //-s protocolStackName -i interfaceName -p portName -b baudrate lResult = 1; break; case 'd': g_deviceName2 = optarg; //this is a special break; case 's': g_protocolStackName2 = optarg; break; case 'i': g_interfaceName2 = optarg; break; case 'p': g_portName2 = optarg; break; case 'b': g_baudrate2 = atoi(optarg); break; case 'n': g_usNodeId2 = (unsigned short)atoi(optarg); break; case '?': // unknown option... stringstream msg; msg << "Unknown option: '" << char(optopt) << "'!"; LogInfo(msg.str()); PrintUsage(); lResult = MMC_FAILED; break; } } return lResult; } int DemoProfilePositionMode(HANDLE p_DeviceHandle, unsigned short p_usNodeId, unsigned int & p_rlErrorCode) { int lResult = MMC_SUCCESS; stringstream msg; msg << "set profile position mode, node = " << p_usNodeId; LogInfo(msg.str()); if(VCS_ActivateProfilePositionMode(p_DeviceHandle, p_usNodeId, &p_rlErrorCode) == 0) { LogError("VCS_ActivateProfilePositionMode", lResult, p_rlErrorCode); lResult = MMC_FAILED; } else { list<long> positionList; positionList.push_back(5000); positionList.push_back(-20000); positionList.push_back(20000); positionList.push_back(-10000); positionList.push_back(10000); positionList.push_back(-10000); positionList.push_back(5000); for(list<long>::iterator it = positionList.begin(); it !=positionList.end(); it++) { long targetPosition = (*it); stringstream msg; msg << "move to position = " << targetPosition << ", node = " << p_usNodeId; LogInfo(msg.str()); if(VCS_MoveToPosition(p_DeviceHandle, p_usNodeId, targetPosition, 0, 1, &p_rlErrorCode) == 0) { LogError("VCS_MoveToPosition", lResult, p_rlErrorCode); lResult = MMC_FAILED; break; } sleep(1); } if(lResult == MMC_SUCCESS) { LogInfo("halt position movement"); if(VCS_HaltPositionMovement(p_DeviceHandle, p_usNodeId, &p_rlErrorCode) == 0) { LogError("VCS_HaltPositionMovement", lResult, p_rlErrorCode); lResult = MMC_FAILED; } } } return lResult; } bool DemoProfileVelocityMode(HANDLE p_DeviceHandle, unsigned short p_usNodeId, unsigned int & p_rlErrorCode) { int lResult = MMC_SUCCESS; stringstream msg; msg << "set profile velocity mode, node = " << p_usNodeId; LogInfo(msg.str()); if(VCS_ActivateProfileVelocityMode(p_DeviceHandle, p_usNodeId, &p_rlErrorCode) == 0) { LogError("VCS_ActivateProfileVelocityMode", lResult, p_rlErrorCode); lResult = MMC_FAILED; } else { list<long> velocityList; velocityList.push_back(500); velocityList.push_back(300); velocityList.push_back(600); velocityList.push_back(900); velocityList.push_back(200); for(list<long>::iterator it = velocityList.begin(); it !=velocityList.end(); it++) { long targetvelocity = (*it); //mubiaosudu stringstream msg; msg << "move with target velocity = " << targetvelocity << " rpm, node = " << p_usNodeId; LogInfo(msg.str()); if(VCS_MoveWithVelocity(p_DeviceHandle, p_usNodeId, targetvelocity, &p_rlErrorCode) == 0) { lResult = MMC_FAILED; LogError("VCS_MoveWithVelocity", lResult, p_rlErrorCode); break; } sleep(3); } if(lResult == MMC_SUCCESS) { LogInfo("halt velocity movement"); if(VCS_HaltVelocityMovement(p_DeviceHandle, p_usNodeId, &p_rlErrorCode) == 0) { lResult = MMC_FAILED; LogError("VCS_HaltVelocityMovement", lResult, p_rlErrorCode); } } } return lResult; } int PrepareDemo(unsigned int* p_pErrorCode) { int lResult = MMC_SUCCESS; BOOL oIsFault = 0; if(VCS_GetFaultState(g_pKeyHandle2, g_usNodeId2, &oIsFault, p_pErrorCode ) == 0) { LogError("VCS_GetFaultState", lResult, *p_pErrorCode); lResult = MMC_FAILED; } if(lResult==0) { if(oIsFault) { stringstream msg; msg << "clear fault, node = '" << g_usNodeId2 << "'"; LogInfo(msg.str()); if(VCS_ClearFault(g_pKeyHandle2, g_usNodeId2, p_pErrorCode) == 0) { LogError("VCS_ClearFault", lResult, *p_pErrorCode); lResult = MMC_FAILED; } } if(lResult==0) { BOOL oIsEnabled = 0; if(VCS_GetEnableState(g_pKeyHandle2, g_usNodeId2, &oIsEnabled, p_pErrorCode) == 0) { LogError("VCS_GetEnableState", lResult, *p_pErrorCode); lResult = MMC_FAILED; } if(lResult==0) { if(!oIsEnabled) { if(VCS_SetEnableState(g_pKeyHandle2, g_usNodeId2, p_pErrorCode) == 0) { LogError("VCS_SetEnableState", lResult, *p_pErrorCode); lResult = MMC_FAILED; } } } } } return lResult; } int Demo(unsigned int* p_pErrorCode) { int lResult = MMC_SUCCESS; unsigned int lErrorCode = 0; lResult = DemoProfileVelocityMode(g_pKeyHandle2, g_usNodeId2, lErrorCode); if(lResult != MMC_SUCCESS) { LogError("DemoProfileVelocityMode", lResult, lErrorCode); } else { lResult = DemoProfilePositionMode(g_pKeyHandle2, g_usNodeId2, lErrorCode); if(lResult != MMC_SUCCESS) { LogError("DemoProfilePositionMode", lResult, lErrorCode); } else { if(VCS_SetDisableState(g_pKeyHandle2, g_usNodeId2, &lErrorCode) == 0) { LogError("VCS_SetDisableState", lResult, lErrorCode); lResult = MMC_FAILED; } } } return lResult; } void PrintHeader() //can clear that..------------------------------------------------------ { // Epos Command Library Example Program, (c) maxonmotor ag 2014-2017 SeparatorLine(); //--------------------------------------------------------------- LogInfo("Epos Command Library Example Program, (c) maxonmotor ag 2014-2017"); SeparatorLine(); } int main(int argc, char argv) { int lResult = MMC_FAILED; unsigned int ulErrorCode = 0; PrintHeader(); SetDefaultParameters(); if((lResult = ParseArguments(argc, argv))!=MMC_SUCCESS) { return lResult; } PrintSettings(); //output *efault settings: example protocal stack name = 'MAXON_RS232 if((lResult = OpenDevice(&ulErrorCode))!=MMC_SUCCESS) { LogError("OpenDevice", lResult, ulErrorCode); return lResult; } if((lResult = PrepareDemo(&ulErrorCode))!=MMC_SUCCESS) { LogError("PrepareDemo", lResult, ulErrorCode); return lResult; } if((lResult = Demo(&ulErrorCode))!=MMC_SUCCESS) { LogError("Demo", lResult, ulErrorCode); return lResult; } if((lResult = CloseDevice(&ulErrorCode))!=MMC_SUCCESS) { LogError("CloseDevice", lResult, ulErrorCode); return lResult; } return lResult; }

版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容,请联系我们,一经查实,本站将立刻删除。
如需转载请保留出处:https://51itzy.com/kjqy/78625.html