TFmini Plus 接入 Linux / Python / Android CAN / UART：开发示例

适用范围

TFmini Plus · Linux / Python / Android · CAN、UART

原始英文技术说明

CMakeLists.txt

cmakeminimumrequired(VERSION 2.8.3) project(tfplus_ros)

Compile as C++11, supported in ROS Kinetic and newer # addcompileoptions(-std=c++11)

Find catkin macros and libraries ## if COMPONENTS list like findpackage(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages findpackage(catkin REQUIRED COMPONENTS roscpp sensormsgs ) findpackage(Boost REQUIRED COMPONENTS system)

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/userguide/setupdotpy.html # catkinpython_setup()

################################################ ## Declare ROS messages, services and actions ## ################################################

To declare and build messages, services or actions from within this ## package, follow these steps: ## Let MSGDEPSET be the set of packages whose message types you use in ## your messages/services/actions (e.g. stdmsgs, actionlibmsgs, ...). ## In the file package.xml: ## add a builddepend tag for "messagegeneration" ## add a builddepend and a execdepend tag for each package in MSGDEPSET ## If MSGDEPSET isn't empty the following dependency has been pulled in ## but can be declared for certainty nonetheless: ## add a execdepend tag for "messageruntime" ## In this file (CMakeLists.txt): ## add "messagegeneration" and every package in MSGDEPSET to ## findpackage(catkin REQUIRED COMPONENTS ...) ## add "messageruntime" and every package in MSGDEPSET to ## catkinpackage(CATKINDEPENDS ...) ## uncomment the addfiles sections below as needed ## and list every .msg/.srv/.action file to be processed ## uncomment the generatemessages entry below ## * add every package in MSGDEPSET to generate_messages(DEPENDENCIES ...)

Generate messages in the 'msg' folder # addmessagefiles( # FILES # Message1.msg # Message2.msg # )

Generate services in the 'srv' folder # addservicefiles( # FILES # Service1.srv # Service2.srv # )

Generate actions in the 'action' folder # addactionfiles( # FILES # Action1.action # Action2.action # )

Generate added messages and services with any dependencies listed here # generatemessages( # DEPENDENCIES # sensormsgs # )

################################################ ## 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 builddepend and a execdepend tag for "dynamicreconfigure" ## In this file (CMakeLists.txt): ## add "dynamicreconfigure" to ## findpackage(catkin REQUIRED COMPONENTS ...) ## * uncomment the "generatedynamicreconfigureoptions" section below ## and list every .cfg file to be processed

Generate dynamic reconfigure parameters in the 'cfg' folder # generatedynamicreconfigure_options( # cfg/DynReconf1.cfg # cfg/DynReconf2.cfg # )

################################### ## catkin specific configuration ## ################################### ## The catkinpackage macro generates cmake config files for your package ## Declare things to be passed to dependent projects ## INCLUDEDIRS: uncomment this if your package contains header files ## LIBRARIES: libraries you create in this project that dependent projects also need ## CATKINDEPENDS: catkinpackages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkinpackage( INCLUDEDIRS include # LIBRARIES tfplusros # CATKINDEPENDS roscpp sensormsgs # DEPENDS systemlib )

########### ## Build ## ###########

Specify additional locations of header files ## Your package locations should be listed before other locations includedirectories( include ${catkinINCLUDE_DIRS} )

Declare a C++ library # addlibrary(${PROJECTNAME} # src/${PROJECTNAME}/tfplusros.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 # adddependencies(${PROJECTNAME} ${${PROJECTNAME}EXPORTEDTARGETS} ${catkinEXPORTED_TARGETS})

Declare a C++ executable ## With catkinmake all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide addexecutable(tfplusrosnode src/TFplusrosnode.cpp src/TFplus.cpp )

Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the ## target back to the shorter version for ease of user use ## e.g. "rosrun someonespkg node" instead of "rosrun someonespkg someonespkgnode" # settargetproperties(${PROJECTNAME}node PROPERTIES OUTPUT_NAME node PREFIX "")

Add cmake target dependencies of the executable ## same as for the library above # adddependencies(${PROJECTNAME}node ${${PROJECTNAME}EXPORTEDTARGETS} ${catkinEXPORTEDTARGETS})

Specify libraries to link a library or executable target against targetlinklibraries(tfplusrosnode ${catkin_LIBRARIES} )

set(CMAKECXXFLAGS "${CMAKECXXFLAGS} -std=c++11 -std=gnu++11")

############# ## Install ## #############

# all install targets should use catkin DESTINATION variables # See http://ros.org/doc/api/catkin/html/advuserguide/variables.html

Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination # install(PROGRAMS # scripts/mypythonscript # DESTINATION ${CATKINPACKAGEBIN_DESTINATION} # )

Mark executables and/or libraries for installation install(TARGETS tfplusrosnode tfplusrosnode # ARCHIVE DESTINATION ${CATKINPACKAGELIBDESTINATION} # LIBRARY DESTINATION ${CATKINPACKAGELIBDESTINATION} RUNTIME DESTINATION ${CATKINPACKAGEBIN_DESTINATION} )

Mark cpp header files for installation install(DIRECTORY include DESTINATION ${CATKINPACKAGEINCLUDEDESTINATION} FILESMATCHING PATTERN "*.h" # PATTERN ".svn" EXCLUDE )

Mark other files for installation (e.g. launch and bag files, etc.) install(FILES launch/tfplus.launch # # myfile1 # # myfile2 DESTINATION ${CATKINPACKAGESHARE_DESTINATION} )

############# ## Testing ## #############

Add gtest based cpp test target and link libraries # catkinaddgtest(${PROJECTNAME}-test test/testtfplusros.cpp) # if(TARGET ${PROJECTNAME}-test) # targetlinklibraries(${PROJECTNAME}-test ${PROJECTNAME}) # endif()

Add folders to be run by python nosetests # catkinaddnosetests(test)

README.txt

/**************************************************************************************************************************************
/* Benewake TFplus infrared range sensor TFplus ROS package
/* Version 1.0
/**************************************************************************************************************************************

/* / Package Information /** Package: tfplusros Node: tfplusrosnode Published Topics: /tfplusrosnode/TFplus (sensormsgs/Range) The distance of object detected. Note: This node won't publish topic if no object exists within TFplus's measurement range, and the behavior can be changed in file /src/TFplusrosnode.cpp

/**************************************************************************************************************************************
/* Quick Start
/**************************************************************************************************************************************
$ cd tfplus_ros/src
$ catkin_make
$ roslaunch tfplus_ros tfplus.launch

TFplus.h

#include <ros/ros.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <errno.h>
#include <string>
#include <sensor_msgs/Range.h>

namespace benewake { class TFplus { public: TFplus(std::string name, int baudRate); ~TFplus(){}; float getDist(); void closePort();

unsigned char dataBuf[7];

private:
std::string portName_;
int baudRate_;
int serial_;
bool readData(unsigned char *_buf, int _nRead);
};
}

tfplus.launch

<launch> <node pkg="tfplusros" type="tfplusrosnode" name="tfplusrosnode"> <param name="serialport" type="string" value="/dev/ttyUSB0" /> <param name="baud_rate" type="int" value="115200" /> </node>

<node pkg="tf" type="statictransformpublisher" name="broadcasterTFplus" args="0 0 0 0 0 0 base_link TFplus 100" /> </launch>

package.xml

<?xml version="1.0"?> <package format="2"> <name>tfplusros</name> <version>0.0.0</version> <description>The tfplusros package</description>

<!-- One maintainer tag required, multiple allowed, one person per tag --> <!-- Example: --> <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --> <maintainer email="ibrahim@yahoo.com">ibrahim</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/tfplus_ros</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: --> <!-- <builddepend>roscpp</builddepend> --> <!-- <execdepend>roscpp</execdepend> --> <!-- Use builddepend for packages you need at compile time: --> <!-- <builddepend>messagegeneration</builddepend> --> <!-- Use buildexportdepend for packages you need in order to build against this package: --> <!-- <buildexportdepend>messagegeneration</buildexportdepend> --> <!-- Use buildtooldepend for build tool packages: --> <!-- <buildtooldepend>catkin</buildtooldepend> --> <!-- Use execdepend for packages you need at runtime: --> <!-- <execdepend>messageruntime</execdepend> --> <!-- Use testdepend for packages you need only for testing: --> <!-- <testdepend>gtest</testdepend> --> <!-- Use docdepend for packages you need only for building documentation: --> <!-- <docdepend>doxygen</docdepend> --> <buildtooldepend>catkin</buildtooldepend> <builddepend>roscpp</builddepend> <builddepend>sensormsgs</builddepend> <buildexportdepend>roscpp</buildexportdepend> <buildexportdepend>sensormsgs</buildexportdepend> <execdepend>roscpp</execdepend> <execdepend>sensormsgs</execdepend> <execdepend>boost</exec_depend>

<!-- The export tag contains other, unspecified, tags --> <export> <!-- Other tools can request additional information be placed here -->

</export> </package>

TFplus.cpp

#include <TFplus.h>
#include <ros/ros.h>
//#define DEBUG
namespace benewake
{
TFplus::TFplus(std::string _name, int _baudRate) :
portName_(_name), baudRate_(_baudRate)
{
serial_ = open(_name.c_str(), O_RDWR);
if(serial_ == -1)
{
ROS_ERROR_STREAM("Failed to open serial port!");
exit(0);
}

struct termios options; if(tcgetattr(serial, &options) != 0){ ROSERRORSTREAM("Can't get serial port sets!"); exit(0); } tcflush(serial, TCIFLUSH); switch(baudRate) { case 921600: cfsetispeed(&options, B921600); cfsetospeed(&options, B921600); break; case 576000: cfsetispeed(&options, B576000); cfsetospeed(&options, B576000); break; case 500000: cfsetispeed(&options, B500000); cfsetospeed(&options, B500000); break; case 460800: cfsetispeed(&options, B460800); cfsetospeed(&options, B460800); break; case 230400: cfsetispeed(&options, B230400); cfsetospeed(&options, B230400); break; case 115200: cfsetispeed(&options, B115200); cfsetospeed(&options, B115200); break; case 57600: cfsetispeed(&options, B57600); cfsetospeed(&options, B57600); break; case 38400: cfsetispeed(&options, B38400); cfsetospeed(&options, B38400); break; case 19200: cfsetispeed(&options, B19200); cfsetospeed(&options, B19200); break; case 9600: cfsetispeed(&options, B9600); cfsetospeed(&options, B9600); break; case 4800: cfsetispeed(&options, B4800); cfsetospeed(&options, B4800); break; default: ROSERRORSTREAM("Unsupported baud rate!"); exit(0); } options.clflag &= ~(ICANON | ECHO | ECHOE | ISIG); options.coflag &= ~OPOST; options.ciflag &= ~(IXON | IXOFF | IXANY); options.ciflag &= ~(INLCR | ICRNL | IGNCR); options.coflag &= ~(ONLCR | OCRNL); if(tcsetattr(serial, TCSANOW, &options) != 0){ ROSERROR_STREAM("Can't set serial port options!"); exit(0); } }

bool TFplus::readData(unsigned char* _buf, int _nRead)
{
int total = 0, ret = 0;
while(total != _nRead)
{
ret = read(serial_, _buf + total, (_nRead - total));
if(ret < 0)
{
return false;
}
total += ret;
}
#ifdef DEBUG
for(int i = 0; i < _nRead; i++)
{
printf("%02x ", _buf[i]);
}
printf("\n");
#endif // DEBUG
return true;
}
float TFplus::getDist()
{
while(true)
{
if(readData(dataBuf, 2))
{
if(dataBuf[0] == 0x59 && dataBuf[1] == 0x59)
break;
}
else
{
return -1.0;
}
}
if(readData(dataBuf, 7))
{
int sumCheck = (0x59 + 0x59) % 256;
for(int i = 0; i < 6; i++)
{
sumCheck = (sumCheck + dataBuf[i]) % 256;
}
if(sumCheck == dataBuf[6])
{
return ((float)(dataBuf[1] << 8 | dataBuf[0]) / 100.0);
}
else
{
return 0.0;
}
}
else
{
return -1.0;
}
}
void TFplus::closePort()
{
close(serial_);
}
}

TFplusrosnode.cpp

#include <TFplus.h>

int main(int argc, char *argv) { ros::init(argc, argv, "tfplusrosnode"); ros::NodeHandle nh("~"); std::string id = "TFplus"; std::string portName; int baudrate; benewake::TFplus tfplusobj;

nh.param("serialport", portName, std::string("/dev/ttyUSB0")); nh.param("baudrate", baud_rate, 115200);

tfplusobj = new benewake::TFplus(portName, baudrate); ros::Publisher pubrange = nh.advertise<sensormsgs::Range>(id, 1000, true); sensormsgs::Range TFplusrange; TFplusrange.radiationtype = sensormsgs::Range::INFRARED; TFplusrange.fieldofview = 0.0349066; // In degrees it is 2 is it's converted into radians. TFplusrange.minrange = 0.2; TFplusrange.maxrange = 8; TFplusrange.header.frameid = id; float dist = 0; ROSINFOSTREAM("Start processing ...");

while(ros::master::check() && ros::ok())
{
ros::spinOnce();
dist = tfplus_obj->getDist();
if(dist > 0 && dist < TFplus_range.max_range)
{
TFplus_range.range = dist;
TFplus_range.header.stamp = ros::Time::now();
pub_range.publish(TFplus_range);
}
else if(dist == -1.0)
{
ROS_ERROR_STREAM("Failed to read data. TFplus ros node stopped!");
break;
}
else if(dist == 0.0)
{
ROS_ERROR_STREAM("Data validation error!");
}
}
tfplus_obj->closePort();
}

使用说明

按原始资料可读取的技术内容编排；具体的产品硬件版本、飞控/软件版本和现场安全边界仍应以原始资料及目标平台当前文档为准。