zqjia
/
point-cloud-visualization
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

颜色方案和雷达显示

pull/1/MERGE
wubw1656 2023-09-15 20:40:34 +08:00
parent ee2d685053
commit 7aff8b40d2
59 changed files with 10688 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt Symbolic link
View File

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

44
librviz_tutorial/CHANGELOG.rst Executable file
View File

@ -0,0 +1,44 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package librviz_tutorial
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.11.0 (2020-05-13)
-------------------
0.10.4 (2020-05-13)
-------------------
* Updated required CMake version to avoid CMP0048 warning (`#57 <https://github.com/ros-visualization/visualization_tutorials/issues/57>`_)
* Contributors: Alejandro Hernández Cordero
0.10.3 (2018-05-09)
-------------------
0.10.2 (2018-01-05)
-------------------
* Unified find_package for Qt4 and Qt5. (`#33 <https://github.com/ros-visualization/visualization_tutorials//issues/33>`_)
* Contributors: Robert Haschke, William Woodall
0.10.1 (2016-04-21)
-------------------
* Added qt5 dependencies to the package.xml.
* Contributors: William Woodall
0.10.0 (2016-04-21)
-------------------
* Added support Qt5 in Kinetic.
* Contributors: William Woodall
0.9.2 (2015-09-21)
------------------
0.9.1 (2015-01-26)
------------------
* Renamed a CMake variable to avoid colliding with built-in name.
* librviz_tutorial now installs it's executable ``myviz``.
* Removed explicit default_plugin library to fix "ld: cannot find -ldefault_plugin" isolated build error
* Contributors: Honore Doktorr, Kei Okada, William Woodall
0.9.0 (2014-03-24)
------------------
* set myself (william) as maintainer
* Contributors: William Woodall

95
librviz_tutorial/CMakeLists.txt Executable file
View File

@ -0,0 +1,95 @@
## BEGIN_TUTORIAL
## This CMakeLists.txt file for rviz_plugin_tutorials builds both the
## TeleopPanel tutorial and the ImuDisplay tutorial.
##
## First start with some standard catkin stuff.
cmake_minimum_required(VERSION 3.0.2)
project(librviz_tutorial)
set(SDK_PATH "./sdk/")
FILE(GLOB SDK_SRC
"${SDK_PATH}/src/arch/linux/*.cpp"
"${SDK_PATH}/src/hal/*.cpp"
"${SDK_PATH}/src/*.cpp"
)
find_package(catkin REQUIRED COMPONENTS
rviz roscpp std_msgs
rosconsole sensor_msgs # node.cpp
)
catkin_package()
include_directories(
${catkin_INCLUDE_DIRS}
${SDK_PATH}/include
${SDK_PATH}/src
)
link_directories(${catkin_LIBRARY_DIRS})
## This setting causes Qt's "MOC" generation to happen automatically.
set(CMAKE_AUTOMOC ON)
## This plugin includes Qt widgets, so we must include Qt.
## We'll use the version that rviz used so they are compatible.
if(rviz_QT_VERSION VERSION_LESS "5")
message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui)
## pull in all required include dirs, define QT_LIBRARIES, etc.
include(${QT_USE_FILE})
else()
message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets)
## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies
set(QT_LIBRARIES Qt5::Widgets)
endif()
## I prefer the Qt signals and slots to avoid defining "emit", "slots",
## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here.
add_definitions(-DQT_NO_KEYWORDS)
## Here we specify the list of source files.
## The generated MOC files are included automatically as headers.
set(SRC_FILES
src/myviz.cpp
src/main.cpp
tools/color.cpp
)
## Add the "myviz" executable and specify the list of source files we
## collected above in ``${SRC_FILES}``.
add_executable(myviz ${SRC_FILES} )
add_executable(rplidarNode src/node.cpp ${SDK_SRC}) # 创建启动laser
target_link_libraries(rplidarNode ${catkin_LIBRARIES})
## Link the myviz executable with whatever Qt libraries have been defined by
## the ``find_package(Qt4 ...)`` line above, or by the
## ``set(QT_LIBRARIES Qt5::Widgets)``, and with whatever libraries
## catkin has included.
target_link_libraries(myviz ${QT_LIBRARIES} ${catkin_LIBRARIES})
## END_TUTORIAL
## Install
install(TARGETS myviz DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
install(TARGETS rplidarNode
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY launch sdk
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
USE_SOURCE_PERMISSIONS
)

10
librviz_tutorial/launch/laser.launch Normal file
View File

@ -0,0 +1,10 @@
<!--
Used for visualising rplidar in action.
It requires rplidar.launch.
-->
<launch>
<include file="$(find librviz_tutorial)/launch/rplidar.launch" />
<node name="myviz" pkg="librviz_tutorial" type="myviz"/>
</launch>

10
librviz_tutorial/launch/rplidar.launch Executable file
View File

@ -0,0 +1,10 @@
<launch>
<node name="rplidarNode" pkg="librviz_tutorial" type="rplidarNode" output="screen">
<param name="serial_port" type="string" value="/dev/ttyUSB0"/>
<param name="serial_baudrate" type="int" value="115200"/><!--A1/A2 -->
<!--param name="serial_baudrate" type="int" value="256000"--><!--A3 -->
<param name="frame_id" type="string" value="laser"/>
<param name="inverted" type="bool" value="false"/>
<param name="angle_compensate" type="bool" value="true"/>
</node>
</launch>

30
librviz_tutorial/package.xml Executable file
View File

@ -0,0 +1,30 @@
<package>
<name>librviz_tutorial</name>
<version>0.11.0</version>
<description>
Tutorial showing how to compile your own C++ program with RViz displays and features.
</description>
<maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>
<license>BSD</license>
<url>http://ros.org/wiki/librviz_tutorial</url>
<author>Dave Hershberger</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>qtbase5-dev</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rviz</build_depend>
<run_depend>libqt5-core</run_depend>
<run_depend>libqt5-gui</run_depend>
<run_depend>libqt5-widgets</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rviz</run_depend>
<run_depend>std_msgs</run_depend>
<export>
<rosdoc config="${prefix}/rosdoc.yaml"/>
</export>
</package>

2
librviz_tutorial/rosdoc.yaml Executable file
View File

@ -0,0 +1,2 @@
- builder: sphinx
sphinx_root_dir: src/doc

34
librviz_tutorial/sdk/README.txt Executable file
View File

@ -0,0 +1,34 @@
Copyright (c) 2009 - 2014 RoboPeak Team
Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* 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.
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.
This folder contains RPLIDAR SDK source code which is provided by RoboPeak.
RoboPeak Website: http://www.robopeak.com
SlamTec HomePage: http://www.slamtec.com
RPLIDAR_SDK_VERSION: 1.9.0
Note: The SDK version may not up-to-date.
rplidar product: http://www.slamtec.com/en/Lidar

44
librviz_tutorial/sdk/include/rplidar.h Executable file
View File

@ -0,0 +1,44 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
#include <vector>
#include "hal/types.h"
#include "rplidar_protocol.h"
#include "rplidar_cmd.h"
#include "rplidar_driver.h"
#define RPLIDAR_SDK_VERSION "1.9.0"

285
librviz_tutorial/sdk/include/rplidar_cmd.h Executable file
View File

@ -0,0 +1,285 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
#include "rplidar_protocol.h"
// Commands
//-----------------------------------------
// Commands without payload and response
#define RPLIDAR_CMD_STOP 0x25
#define RPLIDAR_CMD_SCAN 0x20
#define RPLIDAR_CMD_FORCE_SCAN 0x21
#define RPLIDAR_CMD_RESET 0x40
// Commands without payload but have response
#define RPLIDAR_CMD_GET_DEVICE_INFO 0x50
#define RPLIDAR_CMD_GET_DEVICE_HEALTH 0x52
#define RPLIDAR_CMD_GET_SAMPLERATE 0x59 //added in fw 1.17
#define RPLIDAR_CMD_HQ_MOTOR_SPEED_CTRL 0xA8
// Commands with payload and have response
#define RPLIDAR_CMD_EXPRESS_SCAN 0x82 //added in fw 1.17
#define RPLIDAR_CMD_HQ_SCAN 0x83 //added in fw 1.24
#define RPLIDAR_CMD_GET_LIDAR_CONF 0x84 //added in fw 1.24
#define RPLIDAR_CMD_SET_LIDAR_CONF 0x85 //added in fw 1.24
//add for A2 to set RPLIDAR motor pwm when using accessory board
#define RPLIDAR_CMD_SET_MOTOR_PWM 0xF0
#define RPLIDAR_CMD_GET_ACC_BOARD_FLAG 0xFF
#if defined(_WIN32)
#pragma pack(1)
#endif
// Payloads
// ------------------------------------------
#define RPLIDAR_EXPRESS_SCAN_MODE_NORMAL 0
#define RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE 0 // won't been supported but keep to prevent build fail
//for express working flag(extending express scan protocol)
#define RPLIDAR_EXPRESS_SCAN_FLAG_BOOST 0x0001
#define RPLIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION 0x0002
//for ultra express working flag
#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_STD 0x0001
#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY 0x0002
#define RPLIDAR_HQ_SCAN_FLAG_CCW (0x1<<0)
#define RPLIDAR_HQ_SCAN_FLAG_RAW_ENCODER (0x1<<1)
#define RPLIDAR_HQ_SCAN_FLAG_RAW_DISTANCE (0x1<<2)
typedef struct _rplidar_payload_express_scan_t {
_u8 working_mode;
_u16 working_flags;
_u16 param;
} __attribute__((packed)) rplidar_payload_express_scan_t;
typedef struct _rplidar_payload_hq_scan_t {
_u8 flag;
_u8 reserved[32];
} __attribute__((packed)) rplidar_payload_hq_scan_t;
typedef struct _rplidar_payload_get_scan_conf_t {
_u32 type;
_u8 reserved[32];
} __attribute__((packed)) rplidar_payload_get_scan_conf_t;
#define MAX_MOTOR_PWM 1023
#define DEFAULT_MOTOR_PWM 660
typedef struct _rplidar_payload_motor_pwm_t {
_u16 pwm_value;
} __attribute__((packed)) rplidar_payload_motor_pwm_t;
typedef struct _rplidar_payload_acc_board_flag_t {
_u32 reserved;
} __attribute__((packed)) rplidar_payload_acc_board_flag_t;
// Response
// ------------------------------------------
#define RPLIDAR_ANS_TYPE_DEVINFO 0x4
#define RPLIDAR_ANS_TYPE_DEVHEALTH 0x6
#define RPLIDAR_ANS_TYPE_MEASUREMENT 0x81
// Added in FW ver 1.17
#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED 0x82
#define RPLIDAR_ANS_TYPE_MEASUREMENT_HQ 0x83
// Added in FW ver 1.17
#define RPLIDAR_ANS_TYPE_SAMPLE_RATE 0x15
//added in FW ver 1.23alpha
#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA 0x84
//added in FW ver 1.24
#define RPLIDAR_ANS_TYPE_GET_LIDAR_CONF 0x20
#define RPLIDAR_ANS_TYPE_SET_LIDAR_CONF 0x21
#define RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG 0xFF
#define RPLIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK (0x1)
typedef struct _rplidar_response_acc_board_flag_t {
_u32 support_flag;
} __attribute__((packed)) rplidar_response_acc_board_flag_t;
#define RPLIDAR_STATUS_OK 0x0
#define RPLIDAR_STATUS_WARNING 0x1
#define RPLIDAR_STATUS_ERROR 0x2
#define RPLIDAR_RESP_MEASUREMENT_SYNCBIT (0x1<<0)
#define RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT 2
#define RPLIDAR_RESP_HQ_FLAG_SYNCBIT (0x1<<0)
#define RPLIDAR_RESP_MEASUREMENT_CHECKBIT (0x1<<0)
#define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT 1
typedef struct _rplidar_response_sample_rate_t {
_u16 std_sample_duration_us;
_u16 express_sample_duration_us;
} __attribute__((packed)) rplidar_response_sample_rate_t;
typedef struct _rplidar_response_measurement_node_t {
_u8 sync_quality; // syncbit:1;syncbit_inverse:1;quality:6;
_u16 angle_q6_checkbit; // check_bit:1;angle_q6:15;
_u16 distance_q2;
} __attribute__((packed)) rplidar_response_measurement_node_t;
//[distance_sync flags]
#define RPLIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK (0x3)
#define RPLIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK (0xFC)
typedef struct _rplidar_response_cabin_nodes_t {
_u16 distance_angle_1; // see [distance_sync flags]
_u16 distance_angle_2; // see [distance_sync flags]
_u8 offset_angles_q3;
} __attribute__((packed)) rplidar_response_cabin_nodes_t;
#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1 0xA
#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2 0x5
#define RPLIDAR_RESP_MEASUREMENT_HQ_SYNC 0xA5
#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT (0x1<<15)
typedef struct _rplidar_response_capsule_measurement_nodes_t {
_u8 s_checksum_1; // see [s_checksum_1]
_u8 s_checksum_2; // see [s_checksum_1]
_u16 start_angle_sync_q6;
rplidar_response_cabin_nodes_t cabins[16];
} __attribute__((packed)) rplidar_response_capsule_measurement_nodes_t;
// ext1 : x2 boost mode
#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS 12
#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS 10
typedef struct _rplidar_response_ultra_cabin_nodes_t {
// 31 0
// | predict2 10bit | predict1 10bit | major 12bit |
_u32 combined_x3;
} __attribute__((packed)) rplidar_response_ultra_cabin_nodes_t;
typedef struct _rplidar_response_ultra_capsule_measurement_nodes_t {
_u8 s_checksum_1; // see [s_checksum_1]
_u8 s_checksum_2; // see [s_checksum_1]
_u16 start_angle_sync_q6;
rplidar_response_ultra_cabin_nodes_t ultra_cabins[32];
} __attribute__((packed)) rplidar_response_ultra_capsule_measurement_nodes_t;
typedef struct rplidar_response_measurement_node_hq_t {
_u16 angle_z_q14;
_u32 dist_mm_q2;
_u8 quality;
_u8 flag;
} __attribute__((packed)) rplidar_response_measurement_node_hq_t;
typedef struct _rplidar_response_hq_capsule_measurement_nodes_t{
_u8 sync_byte;
_u64 time_stamp;
rplidar_response_measurement_node_hq_t node_hq[16];
_u32 crc32;
}__attribute__((packed)) rplidar_response_hq_capsule_measurement_nodes_t;
# define RPLIDAR_CONF_SCAN_COMMAND_STD 0
# define RPLIDAR_CONF_SCAN_COMMAND_EXPRESS 1
# define RPLIDAR_CONF_SCAN_COMMAND_HQ 2
# define RPLIDAR_CONF_SCAN_COMMAND_BOOST 3
# define RPLIDAR_CONF_SCAN_COMMAND_STABILITY 4
# define RPLIDAR_CONF_SCAN_COMMAND_SENSITIVITY 5
#define RPLIDAR_CONF_ANGLE_RANGE 0x00000000
#define RPLIDAR_CONF_DESIRED_ROT_FREQ 0x00000001
#define RPLIDAR_CONF_SCAN_COMMAND_BITMAP 0x00000002
#define RPLIDAR_CONF_MIN_ROT_FREQ 0x00000004
#define RPLIDAR_CONF_MAX_ROT_FREQ 0x00000005
#define RPLIDAR_CONF_MAX_DISTANCE 0x00000060
#define RPLIDAR_CONF_SCAN_MODE_COUNT 0x00000070
#define RPLIDAR_CONF_SCAN_MODE_US_PER_SAMPLE 0x00000071
#define RPLIDAR_CONF_SCAN_MODE_MAX_DISTANCE 0x00000074
#define RPLIDAR_CONF_SCAN_MODE_ANS_TYPE 0x00000075
#define RPLIDAR_CONF_SCAN_MODE_TYPICAL 0x0000007C
#define RPLIDAR_CONF_SCAN_MODE_NAME 0x0000007F
#define RPLIDAR_EXPRESS_SCAN_STABILITY_BITMAP 4
#define RPLIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP 5
typedef struct _rplidar_response_get_lidar_conf{
_u32 type;
_u8 payload[0];
}__attribute__((packed)) rplidar_response_get_lidar_conf_t;
typedef struct _rplidar_response_set_lidar_conf{
_u32 result;
}__attribute__((packed)) rplidar_response_set_lidar_conf_t;
typedef struct _rplidar_response_device_info_t {
_u8 model;
_u16 firmware_version;
_u8 hardware_version;
_u8 serialnum[16];
} __attribute__((packed)) rplidar_response_device_info_t;
typedef struct _rplidar_response_device_health_t {
_u8 status;
_u16 error_code;
} __attribute__((packed)) rplidar_response_device_health_t;
// Definition of the variable bit scale encoding mechanism
#define RPLIDAR_VARBITSCALE_X2_SRC_BIT 9
#define RPLIDAR_VARBITSCALE_X4_SRC_BIT 11
#define RPLIDAR_VARBITSCALE_X8_SRC_BIT 12
#define RPLIDAR_VARBITSCALE_X16_SRC_BIT 14
#define RPLIDAR_VARBITSCALE_X2_DEST_VAL 512
#define RPLIDAR_VARBITSCALE_X4_DEST_VAL 1280
#define RPLIDAR_VARBITSCALE_X8_DEST_VAL 1792
#define RPLIDAR_VARBITSCALE_X16_DEST_VAL 3328
#define RPLIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_) \
( (((0x1<<(_BITS_)) - RPLIDAR_VARBITSCALE_X16_DEST_VAL)<<4) + \
((RPLIDAR_VARBITSCALE_X16_DEST_VAL - RPLIDAR_VARBITSCALE_X8_DEST_VAL)<<3) + \
((RPLIDAR_VARBITSCALE_X8_DEST_VAL - RPLIDAR_VARBITSCALE_X4_DEST_VAL)<<2) + \
((RPLIDAR_VARBITSCALE_X4_DEST_VAL - RPLIDAR_VARBITSCALE_X2_DEST_VAL)<<1) + \
RPLIDAR_VARBITSCALE_X2_DEST_VAL - 1)
#if defined(_WIN32)
#pragma pack()
#endif

324
librviz_tutorial/sdk/include/rplidar_driver.h Executable file
View File

@ -0,0 +1,324 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
#ifndef __cplusplus
#error "The RPlidar SDK requires a C++ compiler to be built"
#endif
#ifndef DEPRECATED
#ifdef __GNUC__
#define DEPRECATED(func) func __attribute__ ((deprecated))
#elif defined(_MSC_VER)
#define DEPRECATED(func) __declspec(deprecated) func
#else
#pragma message("WARNING: You need to implement DEPRECATED for this compiler")
#define DEPRECATED(func) func
#endif
#endif
namespace rp { namespace standalone{ namespace rplidar {
struct RplidarScanMode {
_u16 id;
float us_per_sample; // microseconds per sample
float max_distance; // max distance
_u8 ans_type; // the answer type of the scam mode, its value should be RPLIDAR_ANS_TYPE_MEASUREMENT*
char scan_mode[64]; // name of scan mode, max 63 characters
};
enum {
DRIVER_TYPE_SERIALPORT = 0x0,
DRIVER_TYPE_TCP = 0x1,
};
class ChannelDevice
{
public:
virtual bool bind(const char*, uint32_t ) = 0;
virtual bool open() {return true;}
virtual void close() = 0;
virtual void flush() {return;}
virtual bool waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL) = 0;
virtual int senddata(const _u8 * data, size_t size) = 0;
virtual int recvdata(unsigned char * data, size_t size) = 0;
virtual void setDTR() {return;}
virtual void clearDTR() {return;}
virtual void ReleaseRxTx() {return;}
};
class RPlidarDriver {
public:
enum {
DEFAULT_TIMEOUT = 2000, //2000 ms
};
enum {
MAX_SCAN_NODES = 8192,
};
enum {
LEGACY_SAMPLE_DURATION = 476,
};
public:
/// Create an RPLIDAR Driver Instance
/// This interface should be invoked first before any other operations
///
/// \param drivertype the connection type used by the driver.
static RPlidarDriver * CreateDriver(_u32 drivertype = DRIVER_TYPE_SERIALPORT);
/// Dispose the RPLIDAR Driver Instance specified by the drv parameter
/// Applications should invoke this interface when the driver instance is no longer used in order to free memory
static void DisposeDriver(RPlidarDriver * drv);
/// Open the specified serial port and connect to a target RPLIDAR device
///
/// \param port_path the device path of the serial port
/// e.g. on Windows, it may be com3 or \\.\com10
/// on Unix-Like OS, it may be /dev/ttyS1, /dev/ttyUSB2, etc
///
/// \param baudrate the baudrate used
/// For most RPLIDAR models, the baudrate should be set to 115200
///
/// \param flag other flags
/// Reserved for future use, always set to Zero
virtual u_result connect(const char *, _u32, _u32 flag = 0) = 0;
/// Disconnect with the RPLIDAR and close the serial port
virtual void disconnect() = 0;
/// Returns TRUE when the connection has been established
virtual bool isConnected() = 0;
/// Ask the RPLIDAR core system to reset it self
/// The host system can use the Reset operation to help RPLIDAR escape the self-protection mode.
///
/// \param timeout The operation timeout value (in millisecond) for the serial port communication
virtual u_result reset(_u32 timeout = DEFAULT_TIMEOUT) = 0;
// FW1.24
/// Get all scan modes that supported by lidar
virtual u_result getAllSupportedScanModes(std::vector<RplidarScanMode>& outModes, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;
/// Get typical scan mode of lidar
virtual u_result getTypicalScanMode(_u16& outMode, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;
/// Start scan
///
/// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not.
/// \param useTypicalScan Use lidar's typical scan mode or use the compatibility mode (2k sps)
/// \param options Scan options (please use 0)
/// \param outUsedScanMode The scan mode selected by lidar
virtual u_result startScan(bool force, bool useTypicalScan, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL) = 0;
/// Start scan in specific mode
///
/// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not.
/// \param scanMode The scan mode id (use getAllSupportedScanModes to get supported modes)
/// \param options Scan options (please use 0)
/// \param outUsedScanMode The scan mode selected by lidar
virtual u_result startScanExpress(bool force, _u16 scanMode, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL, _u32 timeout = DEFAULT_TIMEOUT) = 0;
/// Retrieve the health status of the RPLIDAR
/// The host system can use this operation to check whether RPLIDAR is in the self-protection mode.
///
/// \param health The health status info returned from the RPLIDAR
///
/// \param timeout The operation timeout value (in millisecond) for the serial port communication
virtual u_result getHealth(rplidar_response_device_health_t & health, _u32 timeout = DEFAULT_TIMEOUT) = 0;
/// Get the device information of the RPLIDAR include the serial number, firmware version, device model etc.
///
/// \param info The device information returned from the RPLIDAR
/// \param timeout The operation timeout value (in millisecond) for the serial port communication
virtual u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT) = 0;
/// Get the sample duration information of the RPLIDAR.
/// DEPRECATED, please use RplidarScanMode::us_per_sample
///
/// \param rateInfo The sample duration information returned from the RPLIDAR
/// \param timeout The operation timeout value (in millisecond) for the serial port communication
DEPRECATED(virtual u_result getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout = DEFAULT_TIMEOUT)) = 0;
/// Set the RPLIDAR's motor pwm when using accessory board, currently valid for A2 only.
///
/// \param pwm The motor pwm value would like to set
virtual u_result setMotorPWM(_u16 pwm) = 0;
/// Start RPLIDAR's motor when using accessory board
virtual u_result startMotor() = 0;
/// Stop RPLIDAR's motor when using accessory board
virtual u_result stopMotor() = 0;
/// Check whether the device support motor control.
/// Note: this API will disable grab.
///
/// \param support Return the result.
/// \param timeout The operation timeout value (in millisecond) for the serial port communication.
virtual u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT) = 0;
/// Calculate RPLIDAR's current scanning frequency from the given scan data
/// DEPRECATED, please use getFrequency(RplidarScanMode, size_t)
///
/// Please refer to the application note doc for details
/// Remark: the calcuation will be incorrect if the specified scan data doesn't contains enough data
///
/// \param inExpressMode Indicate whether the RPLIDAR is in express mode
/// \param count The number of sample nodes inside the given buffer
/// \param frequency The scanning frequency (in HZ) calcuated by the interface.
/// \param is4kmode Return whether the RPLIDAR is working on 4k sample rate mode.
DEPRECATED(virtual u_result getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode)) = 0;
/// Calculate RPLIDAR's current scanning frequency from the given scan data
/// Please refer to the application note doc for details
/// Remark: the calcuation will be incorrect if the specified scan data doesn't contains enough data
///
/// \param scanMode Lidar's current scan mode
/// \param count The number of sample nodes inside the given buffer
virtual u_result getFrequency(const RplidarScanMode& scanMode, size_t count, float & frequency) = 0;
/// Ask the RPLIDAR core system to enter the scan mode(Normal/Express, Express mode is 4k mode)
/// A background thread will be created by the RPLIDAR driver to fetch the scan data continuously.
/// User Application can use the grabScanData() interface to retrieved the scan data cached previous by this background thread.
///
/// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not.
/// \param timeout The operation timeout value (in millisecond) for the serial port communication.
virtual u_result startScanNormal(bool force, _u32 timeout = DEFAULT_TIMEOUT) = 0;
/// Check whether the device support express mode.
/// DEPRECATED, please use getAllSupportedScanModes
///
/// \param support Return the result.
/// \param timeout The operation timeout value (in millisecond) for the serial port communication.
DEPRECATED(virtual u_result checkExpressScanSupported(bool & support, _u32 timeout = DEFAULT_TIMEOUT)) = 0;
/// Ask the RPLIDAR core system to stop the current scan operation and enter idle state. The background thread will be terminated
///
/// \param timeout The operation timeout value (in millisecond) for the serial port communication
virtual u_result stop(_u32 timeout = DEFAULT_TIMEOUT) = 0;
/// Wait and grab a complete 0-360 degree scan data previously received.
/// NOTE: This method only support distance less than 16.38 meters, for longer distance, please use grabScanDataHq
/// The grabbed scan data returned by this interface always has the following charactistics:
///
/// 1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1
/// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan
/// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer.
///
/// \param nodebuffer Buffer provided by the caller application to store the scan data
///
/// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t).
/// Once the interface returns, this parameter will store the actual received data count.
///
/// \param timeout Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely.
///
/// The interface will return RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration.
///
/// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation.
DEPRECATED(virtual u_result grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT)) = 0;
/// Wait and grab a complete 0-360 degree scan data previously received.
/// The grabbed scan data returned by this interface always has the following charactistics:
///
/// 1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1
/// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan
/// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer.
///
/// \param nodebuffer Buffer provided by the caller application to store the scan data
///
/// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t).
/// Once the interface returns, this parameter will store the actual received data count.
///
/// \param timeout Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely.
///
/// The interface will return RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration.
///
/// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation.
virtual u_result grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT) = 0;
/// Ascending the scan data according to the angle value in the scan.
///
/// \param nodebuffer Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData
///
/// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t).
/// Once the interface returns, this parameter will store the actual received data count.
/// The interface will return RESULT_OPERATION_FAIL when all the scan data is invalid.
DEPRECATED(virtual u_result ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count)) = 0;
/// Ascending the scan data according to the angle value in the scan.
///
/// \param nodebuffer Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData
///
/// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t).
/// Once the interface returns, this parameter will store the actual received data count.
/// The interface will return RESULT_OPERATION_FAIL when all the scan data is invalid.
virtual u_result ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count) = 0;
/// Return received scan points even if it's not complete scan
///
/// \param nodebuffer Buffer provided by the caller application to store the scan data
///
/// \param count Once the interface returns, this parameter will store the actual received data count.
///
/// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call.
DEPRECATED(virtual u_result getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count)) = 0;
/// Return received scan points even if it's not complete scan
///
/// \param nodebuffer Buffer provided by the caller application to store the scan data
///
/// \param count Once the interface returns, this parameter will store the actual received data count.
///
/// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call.
virtual u_result getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count) = 0;
virtual ~RPlidarDriver() {}
protected:
RPlidarDriver(){}
public:
ChannelDevice* _chanDev;
};
}}}

72
librviz_tutorial/sdk/include/rplidar_protocol.h Executable file
View File

@ -0,0 +1,72 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
// RP-Lidar Input Packets
#define RPLIDAR_CMD_SYNC_BYTE 0xA5
#define RPLIDAR_CMDFLAG_HAS_PAYLOAD 0x80
#define RPLIDAR_ANS_SYNC_BYTE1 0xA5
#define RPLIDAR_ANS_SYNC_BYTE2 0x5A
#define RPLIDAR_ANS_PKTFLAG_LOOP 0x1
#define RPLIDAR_ANS_HEADER_SIZE_MASK 0x3FFFFFFF
#define RPLIDAR_ANS_HEADER_SUBTYPE_SHIFT (30)
#if defined(_WIN32)
#pragma pack(1)
#endif
typedef struct _rplidar_cmd_packet_t {
_u8 syncByte; //must be RPLIDAR_CMD_SYNC_BYTE
_u8 cmd_flag;
_u8 size;
_u8 data[0];
} __attribute__((packed)) rplidar_cmd_packet_t;
typedef struct _rplidar_ans_header_t {
_u8 syncByte1; // must be RPLIDAR_ANS_SYNC_BYTE1
_u8 syncByte2; // must be RPLIDAR_ANS_SYNC_BYTE2
_u32 size_q30_subtype; // see _u32 size:30; _u32 subType:2;
_u8 type;
} __attribute__((packed)) rplidar_ans_header_t;
#if defined(_WIN32)
#pragma pack()
#endif

116
librviz_tutorial/sdk/include/rptypes.h Executable file
View File

@ -0,0 +1,116 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
#ifdef _WIN32
//fake stdint.h for VC only
typedef signed char int8_t;
typedef unsigned char uint8_t;
typedef __int16 int16_t;
typedef unsigned __int16 uint16_t;
typedef __int32 int32_t;
typedef unsigned __int32 uint32_t;
typedef __int64 int64_t;
typedef unsigned __int64 uint64_t;
#else
#include <stdint.h>
#endif
//based on stdint.h
typedef int8_t _s8;
typedef uint8_t _u8;
typedef int16_t _s16;
typedef uint16_t _u16;
typedef int32_t _s32;
typedef uint32_t _u32;
typedef int64_t _s64;
typedef uint64_t _u64;
#define __small_endian
#ifndef __GNUC__
#define __attribute__(x)
#endif
// The _word_size_t uses actual data bus width of the current CPU
#ifdef _AVR_
typedef _u8 _word_size_t;
#define THREAD_PROC
#elif defined (WIN64)
typedef _u64 _word_size_t;
#define THREAD_PROC __stdcall
#elif defined (WIN32)
typedef _u32 _word_size_t;
#define THREAD_PROC __stdcall
#elif defined (__GNUC__)
typedef unsigned long _word_size_t;
#define THREAD_PROC
#elif defined (__ICCARM__)
typedef _u32 _word_size_t;
#define THREAD_PROC
#endif
typedef uint32_t u_result;
#define RESULT_OK 0
#define RESULT_FAIL_BIT 0x80000000
#define RESULT_ALREADY_DONE 0x20
#define RESULT_INVALID_DATA (0x8000 | RESULT_FAIL_BIT)
#define RESULT_OPERATION_FAIL (0x8001 | RESULT_FAIL_BIT)
#define RESULT_OPERATION_TIMEOUT (0x8002 | RESULT_FAIL_BIT)
#define RESULT_OPERATION_STOP (0x8003 | RESULT_FAIL_BIT)
#define RESULT_OPERATION_NOT_SUPPORT (0x8004 | RESULT_FAIL_BIT)
#define RESULT_FORMAT_NOT_SUPPORT (0x8005 | RESULT_FAIL_BIT)
#define RESULT_INSUFFICIENT_MEMORY (0x8006 | RESULT_FAIL_BIT)
#define IS_OK(x) ( ((x) & RESULT_FAIL_BIT) == 0 )
#define IS_FAIL(x) ( ((x) & RESULT_FAIL_BIT) )
typedef _word_size_t (THREAD_PROC * thread_proc_t ) ( void * );

64
librviz_tutorial/sdk/src/arch/linux/arch_linux.h Executable file
View File

@ -0,0 +1,64 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
// libc dep
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <time.h>
#include <stdarg.h>
// libc++ dep
#include <iostream>
#include <string>
// linux specific
#include <unistd.h>
#include <errno.h>
#include <pthread.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/select.h>
#include <time.h>
#include "timer.h"

476
librviz_tutorial/sdk/src/arch/linux/net_serial.cpp Executable file
View File

@ -0,0 +1,476 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#include "arch/linux/arch_linux.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <assert.h>
// linux specific
#include <errno.h>
#include <fcntl.h>
#include <time.h>
#include "hal/types.h"
#include "arch/linux/net_serial.h"
#include <sys/select.h>
#include <algorithm>
//__GNUC__
#if defined(__GNUC__)
// for Linux extension
#include <asm/ioctls.h>
#include <asm/termbits.h>
#include <sys/ioctl.h>
extern "C" int tcflush(int fildes, int queue_selector);
#else
// for other standard UNIX
#include <termios.h>
#include <sys/ioctl.h>
#endif
namespace rp{ namespace arch{ namespace net{
raw_serial::raw_serial()
: rp::hal::serial_rxtx()
, _baudrate(0)
, _flags(0)
, serial_fd(-1)
{
_init();
}
raw_serial::~raw_serial()
{
close();
}
bool raw_serial::open()
{
return open(_portName, _baudrate, _flags);
}
bool raw_serial::bind(const char * portname, uint32_t baudrate, uint32_t flags)
{
strncpy(_portName, portname, sizeof(_portName));
_baudrate = baudrate;
_flags = flags;
return true;
}
bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags)
{
if (isOpened()) close();
serial_fd = ::open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
if (serial_fd == -1) return false;
#if !defined(__GNUC__)
// for standard UNIX
struct termios options, oldopt;
tcgetattr(serial_fd, &oldopt);
bzero(&options,sizeof(struct termios));
// enable rx and tx
options.c_cflag |= (CLOCAL | CREAD);
_u32 termbaud = getTermBaudBitmap(baudrate);
if (termbaud == (_u32)-1) {
close();
return false;
}
cfsetispeed(&options, termbaud);
cfsetospeed(&options, termbaud);
options.c_cflag &= ~PARENB; //no checkbit
options.c_cflag &= ~CSTOPB; //1bit stop bit
options.c_cflag &= ~CRTSCTS; //no flow control
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8; /* Select 8 data bits */
#ifdef CNEW_RTSCTS
options.c_cflag &= ~CNEW_RTSCTS; // no hw flow control
#endif
options.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control
// raw input mode
options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
// raw output mode
options.c_oflag &= ~OPOST;
if (tcsetattr(serial_fd, TCSANOW, &options))
{
close();
return false;
}
#else
// using Linux extension ...
struct termios2 tio;
ioctl(serial_fd, TCGETS2, &tio);
bzero(&tio, sizeof(struct termios2));
tio.c_cflag = BOTHER;
tio.c_cflag |= (CLOCAL | CREAD | CS8); //8 bit no hardware handshake
tio.c_cflag &= ~CSTOPB; //1 stop bit
tio.c_cflag &= ~CRTSCTS; //No CTS
tio.c_cflag &= ~PARENB; //No Parity
#ifdef CNEW_RTSCTS
tio.c_cflag &= ~CNEW_RTSCTS; // no hw flow control
#endif
tio.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control
tio.c_cc[VMIN] = 0; //min chars to read
tio.c_cc[VTIME] = 0; //time in 1/10th sec wait
tio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
// raw output mode
tio.c_oflag &= ~OPOST;
tio.c_ispeed = baudrate;
tio.c_ospeed = baudrate;
ioctl(serial_fd, TCSETS2, &tio);
#endif
tcflush(serial_fd, TCIFLUSH);
if (fcntl(serial_fd, F_SETFL, FNDELAY))
{
close();
return false;
}
_is_serial_opened = true;
_operation_aborted = false;
//Clear the DTR bit to let the motor spin
clearDTR();
do {
// create self pipeline for wait cancellation
if (pipe(_selfpipe) == -1) break;
int flags = fcntl(_selfpipe[0], F_GETFL);
if (flags == -1)
break;
flags |= O_NONBLOCK; /* Make read end nonblocking */
if (fcntl(_selfpipe[0], F_SETFL, flags) == -1)
break;
flags = fcntl(_selfpipe[1], F_GETFL);
if (flags == -1)
break;
flags |= O_NONBLOCK; /* Make write end nonblocking */
if (fcntl(_selfpipe[1], F_SETFL, flags) == -1)
break;
} while (0);
return true;
}
void raw_serial::close()
{
if (serial_fd != -1)
::close(serial_fd);
serial_fd = -1;
if (_selfpipe[0] != -1)
::close(_selfpipe[0]);
if (_selfpipe[1] != -1)
::close(_selfpipe[1]);
_selfpipe[0] = _selfpipe[1] = -1;
_operation_aborted = false;
_is_serial_opened = false;
}
int raw_serial::senddata(const unsigned char * data, size_t size)
{
// FIXME: non-block io should be used
if (!isOpened()) return 0;
if (data == NULL || size ==0) return 0;
size_t tx_len = 0;
required_tx_cnt = 0;
do {
int ans = ::write(serial_fd, data + tx_len, size-tx_len);
if (ans == -1) return tx_len;
tx_len += ans;
required_tx_cnt = tx_len;
}while (tx_len<size);
return tx_len;
}
int raw_serial::recvdata(unsigned char * data, size_t size)
{
if (!isOpened()) return 0;
int ans = ::read(serial_fd, data, size);
if (ans == -1) ans=0;
required_rx_cnt = ans;
return ans;
}
void raw_serial::flush( _u32 flags)
{
tcflush(serial_fd,TCIFLUSH);
}
int raw_serial::waitforsent(_u32 timeout, size_t * returned_size)
{
if (returned_size) *returned_size = required_tx_cnt;
return 0;
}
int raw_serial::waitforrecv(_u32 timeout, size_t * returned_size)
{
if (!isOpened() ) return -1;
if (returned_size) *returned_size = required_rx_cnt;
return 0;
}
int raw_serial::waitfordata(size_t data_count, _u32 timeout, size_t * returned_size)
{
size_t length = 0;
if (returned_size==NULL) returned_size=(size_t *)&length;
*returned_size = 0;
int max_fd;
fd_set input_set;
struct timeval timeout_val;
/* Initialize the input set */
FD_ZERO(&input_set);
FD_SET(serial_fd, &input_set);
if (_selfpipe[0] != -1)
FD_SET(_selfpipe[0], &input_set);
max_fd = std::max<int>(serial_fd, _selfpipe[0]) + 1;
/* Initialize the timeout structure */
timeout_val.tv_sec = timeout / 1000;
timeout_val.tv_usec = (timeout % 1000) * 1000;
if ( isOpened() )
{
if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR;
if (*returned_size >= data_count)
{
return 0;
}
}
while ( isOpened() )
{
/* Do the select */
int n = ::select(max_fd, &input_set, NULL, NULL, &timeout_val);
if (n < 0)
{
// select error
*returned_size = 0;
return ANS_DEV_ERR;
}
else if (n == 0)
{
// time out
*returned_size =0;
return ANS_TIMEOUT;
}
else
{
if (FD_ISSET(_selfpipe[0], &input_set)) {
// require aborting the current operation
int ch;
for (;;) {
if (::read(_selfpipe[0], &ch, 1) == -1) {
break;
}
}
// treat as timeout
*returned_size = 0;
return ANS_TIMEOUT;
}
// data avaliable
assert (FD_ISSET(serial_fd, &input_set));
if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR;
if (*returned_size >= data_count)
{
return 0;
}
else
{
int remain_timeout = timeout_val.tv_sec*1000000 + timeout_val.tv_usec;
int expect_remain_time = (data_count - *returned_size)*1000000*8/_baudrate;
if (remain_timeout > expect_remain_time)
usleep(expect_remain_time);
}
}
}
return ANS_DEV_ERR;
}
size_t raw_serial::rxqueue_count()
{
if ( !isOpened() ) return 0;
size_t remaining;
if (::ioctl(serial_fd, FIONREAD, &remaining) == -1) return 0;
return remaining;
}
void raw_serial::setDTR()
{
if ( !isOpened() ) return;
uint32_t dtr_bit = TIOCM_DTR;
ioctl(serial_fd, TIOCMBIS, &dtr_bit);
}
void raw_serial::clearDTR()
{
if ( !isOpened() ) return;
uint32_t dtr_bit = TIOCM_DTR;
ioctl(serial_fd, TIOCMBIC, &dtr_bit);
}
void raw_serial::_init()
{
serial_fd = -1;
_portName[0] = 0;
required_tx_cnt = required_rx_cnt = 0;
_operation_aborted = false;
_selfpipe[0] = _selfpipe[1] = -1;
}
void raw_serial::cancelOperation()
{
_operation_aborted = true;
if (_selfpipe[1] == -1) return;
::write(_selfpipe[1], "x", 1);
}
_u32 raw_serial::getTermBaudBitmap(_u32 baud)
{
#define BAUD_CONV( _baud_) case _baud_: return B##_baud_
switch (baud) {
BAUD_CONV(1200);
BAUD_CONV(1800);
BAUD_CONV(2400);
BAUD_CONV(4800);
BAUD_CONV(9600);
BAUD_CONV(19200);
BAUD_CONV(38400);
BAUD_CONV(57600);
BAUD_CONV(115200);
BAUD_CONV(230400);
BAUD_CONV(460800);
BAUD_CONV(500000);
BAUD_CONV(576000);
BAUD_CONV(921600);
BAUD_CONV(1000000);
BAUD_CONV(1152000);
BAUD_CONV(1500000);
BAUD_CONV(2000000);
BAUD_CONV(2500000);
BAUD_CONV(3000000);
BAUD_CONV(3500000);
BAUD_CONV(4000000);
}
return -1;
}
}}} //end rp::arch::net
//begin rp::hal
namespace rp{ namespace hal{
serial_rxtx * serial_rxtx::CreateRxTx()
{
return new rp::arch::net::raw_serial();
}
void serial_rxtx::ReleaseRxTx(serial_rxtx *rxtx)
{
delete rxtx;
}
}} //end rp::hal

90
librviz_tutorial/sdk/src/arch/linux/net_serial.h Executable file
View File

@ -0,0 +1,90 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
#include "hal/abs_rxtx.h"
namespace rp{ namespace arch{ namespace net{
class raw_serial : public rp::hal::serial_rxtx
{
public:
enum{
SERIAL_RX_BUFFER_SIZE = 512,
SERIAL_TX_BUFFER_SIZE = 128,
};
raw_serial();
virtual ~raw_serial();
virtual bool bind(const char * portname, uint32_t baudrate, uint32_t flags = 0);
virtual bool open();
virtual void close();
virtual void flush( _u32 flags);
virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL);
virtual int senddata(const unsigned char * data, size_t size);
virtual int recvdata(unsigned char * data, size_t size);
virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL);
virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL);
virtual size_t rxqueue_count();
virtual void setDTR();
virtual void clearDTR();
_u32 getTermBaudBitmap(_u32 baud);
virtual void cancelOperation();
protected:
bool open(const char * portname, uint32_t baudrate, uint32_t flags = 0);
void _init();
char _portName[200];
uint32_t _baudrate;
uint32_t _flags;
int serial_fd;
size_t required_tx_cnt;
size_t required_rx_cnt;
int _selfpipe[2];
bool _operation_aborted;
};
}}}

856
librviz_tutorial/sdk/src/arch/linux/net_socket.cpp Executable file
View File

@ -0,0 +1,856 @@
/*
* RoboPeak Project
* HAL Layer - Socket Interface
* Copyright 2009 - 2013 RoboPeak Project
*
* POXIS Implementation
*/
#include "sdkcommon.h"
#include "../../hal/socket.h"
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <arpa/inet.h>
#include <netdb.h>
namespace rp{ namespace net {
static inline int _halAddrTypeToOSType(SocketAddress::address_type_t type)
{
switch (type) {
case SocketAddress::ADDRESS_TYPE_INET:
return AF_INET;
case SocketAddress::ADDRESS_TYPE_INET6:
return AF_INET6;
case SocketAddress::ADDRESS_TYPE_UNSPEC:
return AF_UNSPEC;
default:
assert(!"should not reach here");
return AF_UNSPEC;
}
}
SocketAddress::SocketAddress()
{
_platform_data = reinterpret_cast<void *>(new sockaddr_storage);
memset(_platform_data, 0, sizeof(sockaddr_storage));
reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
}
SocketAddress::SocketAddress(const SocketAddress & src)
{
_platform_data = reinterpret_cast<void *>(new sockaddr_storage);
memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage));
}
SocketAddress::SocketAddress(const char * addrString, int port, SocketAddress::address_type_t type)
{
_platform_data = reinterpret_cast<void *>(new sockaddr_storage);
memset(_platform_data, 0, sizeof(sockaddr_storage));
// default to ipv4 in case the following operation fails
reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
setAddressFromString(addrString, type);
setPort(port);
}
SocketAddress::SocketAddress(void * platform_data)
: _platform_data(platform_data)
{}
SocketAddress & SocketAddress::operator = (const SocketAddress &src)
{
memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage));
return *this;
}
SocketAddress::~SocketAddress()
{
delete reinterpret_cast<sockaddr_storage *>(_platform_data);
}
SocketAddress::address_type_t SocketAddress::getAddressType() const
{
switch(reinterpret_cast<const sockaddr_storage *>(_platform_data)->ss_family) {
case AF_INET:
return ADDRESS_TYPE_INET;
case AF_INET6:
return ADDRESS_TYPE_INET6;
default:
assert(!"should not reach here");
return ADDRESS_TYPE_INET;
}
}
int SocketAddress::getPort() const
{
switch (getAddressType()) {
case ADDRESS_TYPE_INET:
return (int)ntohs(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_port);
case ADDRESS_TYPE_INET6:
return (int)ntohs(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_port);
default:
return 0;
}
}
u_result SocketAddress::setPort(int port)
{
switch (getAddressType()) {
case ADDRESS_TYPE_INET:
reinterpret_cast<sockaddr_in *>(_platform_data)->sin_port = htons((short)port);
break;
case ADDRESS_TYPE_INET6:
reinterpret_cast<sockaddr_in6 *>(_platform_data)->sin6_port = htons((short)port);
break;
default:
return RESULT_OPERATION_FAIL;
}
return RESULT_OK;
}
u_result SocketAddress::setAddressFromString(const char * address_string, SocketAddress::address_type_t type)
{
int ans = 0;
int prevPort = getPort();
switch (type) {
case ADDRESS_TYPE_INET:
reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
ans = inet_pton(AF_INET,
address_string,
&reinterpret_cast<sockaddr_in *>(_platform_data)->sin_addr);
break;
case ADDRESS_TYPE_INET6:
reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET6;
ans = inet_pton(AF_INET6,
address_string,
&reinterpret_cast<sockaddr_in6 *>(_platform_data)->sin6_addr);
break;
default:
return RESULT_INVALID_DATA;
}
setPort(prevPort);
return ans<=0?RESULT_INVALID_DATA:RESULT_OK;
}
u_result SocketAddress::getAddressAsString(char * buffer, size_t buffersize) const
{
int net_family = reinterpret_cast<const sockaddr_storage *>(_platform_data)->ss_family;
const char *ans = NULL;
switch (net_family) {
case AF_INET:
ans = inet_ntop(net_family, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr,
buffer, buffersize);
break;
case AF_INET6:
ans = inet_ntop(net_family, &reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr,
buffer, buffersize);
break;
}
return ans<=0?RESULT_OPERATION_FAIL:RESULT_OK;
}
size_t SocketAddress::LoopUpHostName(const char * hostname, const char * sevicename, std::vector<SocketAddress> &addresspool , bool performDNS, SocketAddress::address_type_t type)
{
struct addrinfo hints;
struct addrinfo *result;
int ans;
memset(&hints, 0, sizeof(struct addrinfo));
hints.ai_family = _halAddrTypeToOSType(type);
hints.ai_flags = AI_PASSIVE;
if (!performDNS) {
hints.ai_family |= AI_NUMERICSERV | AI_NUMERICHOST;
}
ans = getaddrinfo(hostname, sevicename, &hints, &result);
addresspool.clear();
if (ans != 0) {
// hostname loopup failed
return 0;
}
for (struct addrinfo * cursor = result; cursor != NULL; cursor = cursor->ai_next) {
if (cursor->ai_family == ADDRESS_TYPE_INET || cursor->ai_family == ADDRESS_TYPE_INET6) {
sockaddr_storage * storagebuffer = new sockaddr_storage;
assert(sizeof(sockaddr_storage) >= cursor->ai_addrlen);
memcpy(storagebuffer, cursor->ai_addr, cursor->ai_addrlen);
addresspool.push_back(SocketAddress(storagebuffer));
}
}
freeaddrinfo(result);
return addresspool.size();
}
u_result SocketAddress::getRawAddress(_u8 * buffer, size_t bufferSize) const
{
switch (getAddressType()) {
case ADDRESS_TYPE_INET:
if (bufferSize < sizeof(in_addr::s_addr)) return RESULT_INSUFFICIENT_MEMORY;
memcpy(buffer, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr, sizeof(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr));
break;
case ADDRESS_TYPE_INET6:
if (bufferSize < sizeof(in6_addr::s6_addr)) return RESULT_INSUFFICIENT_MEMORY;
memcpy(buffer, reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr, sizeof(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr));
break;
default:
return RESULT_OPERATION_FAIL;
}
return RESULT_OK;
}
void SocketAddress::setLoopbackAddress(SocketAddress::address_type_t type)
{
int prevPort = getPort();
switch (type) {
case ADDRESS_TYPE_INET:
{
sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
addrv4->sin_family = AF_INET;
addrv4->sin_addr.s_addr = htonl(INADDR_LOOPBACK);
}
break;
case ADDRESS_TYPE_INET6:
{
sockaddr_in6 * addrv6 = reinterpret_cast<sockaddr_in6 *>(_platform_data);
addrv6->sin6_family = AF_INET6;
addrv6->sin6_addr = in6addr_loopback;
}
break;
default:
return;
}
setPort(prevPort);
}
void SocketAddress::setBroadcastAddressIPv4()
{
int prevPort = getPort();
sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
addrv4->sin_family = AF_INET;
addrv4->sin_addr.s_addr = htonl(INADDR_BROADCAST);
setPort(prevPort);
}
void SocketAddress::setAnyAddress(SocketAddress::address_type_t type)
{
int prevPort = getPort();
switch (type) {
case ADDRESS_TYPE_INET:
{
sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
addrv4->sin_family = AF_INET;
addrv4->sin_addr.s_addr = htonl(INADDR_ANY);
}
break;
case ADDRESS_TYPE_INET6:
{
sockaddr_in6 * addrv6 = reinterpret_cast<sockaddr_in6 *>(_platform_data);
addrv6->sin6_family = AF_INET6;
addrv6->sin6_addr = in6addr_any;
}
break;
default:
return;
}
setPort(prevPort);
}
}}
///--------------------------------
namespace rp { namespace arch { namespace net{
using namespace rp::net;
class _single_thread StreamSocketImpl : public StreamSocket
{
public:
StreamSocketImpl(int fd)
: _socket_fd(fd)
{
assert(fd>=0);
int bool_true = 1;
::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR , (char *)&bool_true, sizeof(bool_true) );
enableNoDelay(true);
this->setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH);
}
virtual ~StreamSocketImpl()
{
close(_socket_fd);
}
virtual void dispose()
{
delete this;
}
virtual u_result bind(const SocketAddress & localaddr)
{
const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(localaddr.getPlatformData());
assert(addr);
int ans = ::bind(_socket_fd, addr, sizeof(sockaddr_storage));
if (ans) {
return RESULT_OPERATION_FAIL;
} else {
return RESULT_OK;
}
}
virtual u_result getLocalAddress(SocketAddress & localaddr)
{
struct sockaddr * addr = reinterpret_cast<struct sockaddr *>( const_cast<void *>(localaddr.getPlatformData())); //donnot do this at home...
assert(addr);
size_t actualsize = sizeof(sockaddr_storage);
int ans = ::getsockname(_socket_fd, addr, (socklen_t*)&actualsize);
assert(actualsize <= sizeof(sockaddr_storage));
assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
return ans?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk)
{
int ans;
timeval tv;
tv.tv_sec = timeout / 1000;
tv.tv_usec = (timeout % 1000) * 1000;
if (msk & SOCKET_DIR_RD) {
ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) );
if (ans) return RESULT_OPERATION_FAIL;
}
if (msk & SOCKET_DIR_WR) {
ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv) );
if (ans) return RESULT_OPERATION_FAIL;
}
return RESULT_OK;
}
virtual u_result connect(const SocketAddress & pairAddress)
{
const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(pairAddress.getPlatformData());
int ans = ::connect(_socket_fd, addr, sizeof(sockaddr_storage));
if (!ans) return RESULT_OK;
switch (errno) {
case EAFNOSUPPORT:
return RESULT_OPERATION_NOT_SUPPORT;
#if 0
case EINPROGRESS:
return RESULT_OK; //treat async connection as good status
#endif
case ETIMEDOUT:
return RESULT_OPERATION_TIMEOUT;
default:
return RESULT_OPERATION_FAIL;
}
}
virtual u_result listen(int backlog)
{
int ans = ::listen( _socket_fd, backlog);
return ans?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual StreamSocket * accept(SocketAddress * pairAddress)
{
size_t addrsize;
addrsize = sizeof(sockaddr_storage);
int pair_socket = ::accept( _socket_fd, pairAddress?reinterpret_cast<struct sockaddr *>(const_cast<void *>(pairAddress->getPlatformData())):NULL
, (socklen_t*)&addrsize);
if (pair_socket>=0) {
return new StreamSocketImpl(pair_socket);
} else {
return NULL;
}
}
virtual u_result waitforIncomingConnection(_u32 timeout)
{
return waitforData(timeout);
}
virtual u_result send(const void * buffer, size_t len)
{
size_t ans = ::send( _socket_fd, buffer, len, MSG_NOSIGNAL);
if (ans == (int)len) {
return RESULT_OK;
} else {
switch (errno) {
case EAGAIN:
#if EWOULDBLOCK!=EAGAIN
case EWOULDBLOCK:
#endif
return RESULT_OPERATION_TIMEOUT;
default:
return RESULT_OPERATION_FAIL;
}
}
}
virtual u_result recv(void *buf, size_t len, size_t & recv_len)
{
size_t ans = ::recv( _socket_fd, buf, len, 0);
if (ans == (size_t)-1) {
recv_len = 0;
switch (errno) {
case EAGAIN:
#if EWOULDBLOCK!=EAGAIN
case EWOULDBLOCK:
#endif
return RESULT_OPERATION_TIMEOUT;
default:
return RESULT_OPERATION_FAIL;
}
} else {
recv_len = ans;
return RESULT_OK;
}
}
#if 0
virtual u_result recvNoWait(void *buf, size_t len, size_t & recv_len)
{
size_t ans = ::recv( _socket_fd, buf, len, MSG_DONTWAIT);
if (ans == (size_t)-1) {
recv_len = 0;
if (errno == EAGAIN || errno == EWOULDBLOCK) {
return RESULT_OK;
} else {
return RESULT_OPERATION_FAIL;
}
} else {
recv_len = ans;
return RESULT_OK;
}
}
#endif
virtual u_result getPeerAddress(SocketAddress & peerAddr)
{
struct sockaddr * addr = reinterpret_cast<struct sockaddr *>(const_cast<void *>(peerAddr.getPlatformData())); //donnot do this at home...
assert(addr);
size_t actualsize = sizeof(sockaddr_storage);
int ans = ::getpeername(_socket_fd, addr, (socklen_t*)&actualsize);
assert(actualsize <= sizeof(sockaddr_storage));
assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
return ans?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual u_result shutdown(socket_direction_mask mask)
{
int shutdw_opt ;
switch (mask) {
case SOCKET_DIR_RD:
shutdw_opt = SHUT_RD;
break;
case SOCKET_DIR_WR:
shutdw_opt = SHUT_WR;
break;
case SOCKET_DIR_BOTH:
default:
shutdw_opt = SHUT_RDWR;
}
int ans = ::shutdown(_socket_fd, shutdw_opt);
return ans?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual u_result enableKeepAlive(bool enable)
{
int bool_true = enable?1:0;
return ::setsockopt( _socket_fd, SOL_SOCKET, SO_KEEPALIVE , &bool_true, sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual u_result enableNoDelay(bool enable )
{
int bool_true = enable?1:0;
return ::setsockopt( _socket_fd, IPPROTO_TCP, TCP_NODELAY,&bool_true, sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual u_result waitforSent(_u32 timeout )
{
fd_set wrset;
FD_ZERO(&wrset);
FD_SET(_socket_fd, &wrset);
timeval tv;
tv.tv_sec = timeout / 1000;
tv.tv_usec = (timeout % 1000) * 1000;
int ans = ::select(_socket_fd+1, NULL, &wrset, NULL, &tv);
switch (ans) {
case 1:
// fired
return RESULT_OK;
case 0:
// timeout
return RESULT_OPERATION_TIMEOUT;
default:
delay(0); //relax cpu
return RESULT_OPERATION_FAIL;
}
}
virtual u_result waitforData(_u32 timeout )
{
fd_set rdset;
FD_ZERO(&rdset);
FD_SET(_socket_fd, &rdset);
timeval tv;
tv.tv_sec = timeout / 1000;
tv.tv_usec = (timeout % 1000) * 1000;
int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv);
switch (ans) {
case 1:
// fired
return RESULT_OK;
case 0:
// timeout
return RESULT_OPERATION_TIMEOUT;
default:
delay(0); //relax cpu
return RESULT_OPERATION_FAIL;
}
}
protected:
int _socket_fd;
};
class _single_thread DGramSocketImpl : public DGramSocket
{
public:
DGramSocketImpl(int fd)
: _socket_fd(fd)
{
assert(fd>=0);
int bool_true = 1;
::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR | SO_BROADCAST , (char *)&bool_true, sizeof(bool_true) );
setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH);
}
virtual ~DGramSocketImpl()
{
close(_socket_fd);
}
virtual void dispose()
{
delete this;
}
virtual u_result bind(const SocketAddress & localaddr)
{
const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(localaddr.getPlatformData());
assert(addr);
int ans = ::bind(_socket_fd, addr, sizeof(sockaddr_storage));
if (ans) {
return RESULT_OPERATION_FAIL;
} else {
return RESULT_OK;
}
}
virtual u_result getLocalAddress(SocketAddress & localaddr)
{
struct sockaddr * addr = reinterpret_cast<struct sockaddr *>(const_cast<void *>((localaddr.getPlatformData()))); //donnot do this at home...
assert(addr);
size_t actualsize = sizeof(sockaddr_storage);
int ans = ::getsockname(_socket_fd, addr, (socklen_t*)&actualsize);
assert(actualsize <= sizeof(sockaddr_storage));
assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
return ans?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk)
{
int ans;
timeval tv;
tv.tv_sec = timeout / 1000;
tv.tv_usec = (timeout % 1000) * 1000;
if (msk & SOCKET_DIR_RD) {
ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) );
if (ans) return RESULT_OPERATION_FAIL;
}
if (msk & SOCKET_DIR_WR) {
ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv) );
if (ans) return RESULT_OPERATION_FAIL;
}
return RESULT_OK;
}
virtual u_result waitforSent(_u32 timeout )
{
fd_set wrset;
FD_ZERO(&wrset);
FD_SET(_socket_fd, &wrset);
timeval tv;
tv.tv_sec = timeout / 1000;
tv.tv_usec = (timeout % 1000) * 1000;
int ans = ::select(_socket_fd+1, NULL, &wrset, NULL, &tv);
switch (ans) {
case 1:
// fired
return RESULT_OK;
case 0:
// timeout
return RESULT_OPERATION_TIMEOUT;
default:
delay(0); //relax cpu
return RESULT_OPERATION_FAIL;
}
}
virtual u_result waitforData(_u32 timeout )
{
fd_set rdset;
FD_ZERO(&rdset);
FD_SET(_socket_fd, &rdset);
timeval tv;
tv.tv_sec = timeout / 1000;
tv.tv_usec = (timeout % 1000) * 1000;
int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv);
switch (ans) {
case 1:
// fired
return RESULT_OK;
case 0:
// timeout
return RESULT_OPERATION_TIMEOUT;
default:
delay(0); //relax cpu
return RESULT_OPERATION_FAIL;
}
}
virtual u_result sendTo(const SocketAddress & target, const void * buffer, size_t len)
{
const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(target.getPlatformData());
assert(addr);
size_t ans = ::sendto( _socket_fd, buffer, len, 0, addr, sizeof(sockaddr_storage));
if (ans != (size_t)-1) {
assert(ans == (int)len);
return RESULT_OK;
} else {
switch (errno) {
case EAGAIN:
#if EWOULDBLOCK!=EAGAIN
case EWOULDBLOCK:
#endif
return RESULT_OPERATION_TIMEOUT;
case EMSGSIZE:
return RESULT_INVALID_DATA;
default:
return RESULT_OPERATION_FAIL;
}
}
}
virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr)
{
struct sockaddr * addr = (sourceAddr?reinterpret_cast<struct sockaddr *>(const_cast<void *>(sourceAddr->getPlatformData())):NULL);
size_t source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0);
size_t ans = ::recvfrom( _socket_fd, buf, len, 0, addr, (socklen_t*)&source_addr_size);
if (ans == (size_t)-1) {
recv_len = 0;
switch (errno) {
case EAGAIN:
#if EWOULDBLOCK!=EAGAIN
case EWOULDBLOCK:
#endif
return RESULT_OPERATION_TIMEOUT;
default:
return RESULT_OPERATION_FAIL;
}
} else {
recv_len = ans;
return RESULT_OK;
}
}
#if 0
virtual u_result recvFromNoWait(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr)
{
struct sockaddr * addr = (sourceAddr?reinterpret_cast<struct sockaddr *>(const_cast<void *>(sourceAddr->getPlatformData())):NULL);
size_t source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0);
size_t ans = ::recvfrom( _socket_fd, buf, len, MSG_DONTWAIT, addr, &source_addr_size);
if (ans == (size_t)-1) {
recv_len = 0;
if (errno == EAGAIN || errno == EWOULDBLOCK) {
return RESULT_OK;
} else {
return RESULT_OPERATION_FAIL;
}
} else {
recv_len = ans;
return RESULT_OK;
}
}
#endif
protected:
int _socket_fd;
};
}}}
namespace rp { namespace net{
static inline int _socketHalFamilyToOSFamily(SocketBase::socket_family_t family)
{
switch (family) {
case SocketBase::SOCKET_FAMILY_INET:
return AF_INET;
case SocketBase::SOCKET_FAMILY_INET6:
return AF_INET6;
case SocketBase::SOCKET_FAMILY_RAW:
return AF_PACKET;
default:
assert(!"should not reach here");
return AF_INET; // force treating as IPv4 in release mode
}
}
StreamSocket * StreamSocket::CreateSocket(SocketBase::socket_family_t family)
{
if (family == SOCKET_FAMILY_RAW) return NULL;
int socket_family = _socketHalFamilyToOSFamily(family);
int socket_fd = ::socket(socket_family, SOCK_STREAM, 0);
if (socket_fd == -1) return NULL;
StreamSocket * newborn = static_cast<StreamSocket *>(new rp::arch::net::StreamSocketImpl(socket_fd));
return newborn;
}
DGramSocket * DGramSocket::CreateSocket(SocketBase::socket_family_t family)
{
int socket_family = _socketHalFamilyToOSFamily(family);
int socket_fd = ::socket(socket_family, (family==SOCKET_FAMILY_RAW)?SOCK_RAW:SOCK_DGRAM, 0);
if (socket_fd == -1) return NULL;
DGramSocket * newborn = static_cast<DGramSocket *>(new rp::arch::net::DGramSocketImpl(socket_fd));
return newborn;
}
}}

137
librviz_tutorial/sdk/src/arch/linux/thread.hpp Executable file
View File

@ -0,0 +1,137 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#include "arch/linux/arch_linux.h"
#include <sched.h>
namespace rp{ namespace hal{
Thread Thread::create(thread_proc_t proc, void * data)
{
Thread newborn(proc, data);
// tricky code, we assume pthread_t is not a structure but a word size value
assert( sizeof(newborn._handle) >= sizeof(pthread_t));
pthread_create((pthread_t *)&newborn._handle, NULL, (void * (*)(void *))proc, data);
return newborn;
}
u_result Thread::terminate()
{
if (!this->_handle) return RESULT_OK;
return pthread_cancel((pthread_t)this->_handle)==0?RESULT_OK:RESULT_OPERATION_FAIL;
}
u_result Thread::setPriority( priority_val_t p)
{
if (!this->_handle) return RESULT_OPERATION_FAIL;
// check whether current schedule policy supports priority levels
int current_policy;
struct sched_param current_param;
int ans;
if (pthread_getschedparam( (pthread_t) this->_handle, &current_policy, &current_param))
{
// cannot retreieve values
return RESULT_OPERATION_FAIL;
}
//int pthread_priority = 0 ;
switch(p)
{
case PRIORITY_REALTIME:
//pthread_priority = pthread_priority_max;
current_policy = SCHED_RR;
break;
case PRIORITY_HIGH:
//pthread_priority = (pthread_priority_max + pthread_priority_min)/2;
current_policy = SCHED_RR;
break;
case PRIORITY_NORMAL:
case PRIORITY_LOW:
case PRIORITY_IDLE:
//pthread_priority = 0;
current_policy = SCHED_OTHER;
break;
}
current_param.__sched_priority = current_policy;
if ( (ans = pthread_setschedparam( (pthread_t) this->_handle, current_policy, &current_param)) )
{
return RESULT_OPERATION_FAIL;
}
return RESULT_OK;
}
Thread::priority_val_t Thread::getPriority()
{
if (!this->_handle) return PRIORITY_NORMAL;
int current_policy;
struct sched_param current_param;
if (pthread_getschedparam( (pthread_t) this->_handle, &current_policy, &current_param))
{
// cannot retreieve values
return PRIORITY_NORMAL;
}
int pthread_priority_max = sched_get_priority_max(SCHED_RR);
int pthread_priority_min = sched_get_priority_min(SCHED_RR);
if (current_param.__sched_priority ==(pthread_priority_max ))
{
return PRIORITY_REALTIME;
}
if (current_param.__sched_priority >=(pthread_priority_max + pthread_priority_min)/2)
{
return PRIORITY_HIGH;
}
return PRIORITY_NORMAL;
}
u_result Thread::join(unsigned long timeout)
{
if (!this->_handle) return RESULT_OK;
pthread_join((pthread_t)(this->_handle), NULL);
return RESULT_OK;
}
}}

52
librviz_tutorial/sdk/src/arch/linux/timer.cpp Executable file
View File

@ -0,0 +1,52 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#include "arch/linux/arch_linux.h"
namespace rp{ namespace arch{
_u64 rp_getus()
{
struct timespec t;
t.tv_sec = t.tv_nsec = 0;
clock_gettime(CLOCK_MONOTONIC, &t);
return t.tv_sec*1000000LL + t.tv_nsec/1000;
}
_u32 rp_getms()
{
struct timespec t;
t.tv_sec = t.tv_nsec = 0;
clock_gettime(CLOCK_MONOTONIC, &t);
return t.tv_sec*1000L + t.tv_nsec/1000000L;
}
}}

57
librviz_tutorial/sdk/src/arch/linux/timer.h Executable file
View File

@ -0,0 +1,57 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
#include "hal/types.h"
#include <unistd.h>
static inline void delay(_word_size_t ms){
while (ms>=1000){
usleep(1000*1000);
ms-=1000;
};
if (ms!=0)
usleep(ms*1000);
}
// TODO: the highest timer interface should be clock_gettime
namespace rp{ namespace arch{
_u64 rp_getus();
_u32 rp_getms();
}}
#define getms() rp::arch::rp_getms()

66
librviz_tutorial/sdk/src/arch/macOS/arch_macOS.h Executable file
View File

@ -0,0 +1,66 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
// libc dep
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <time.h>
#include <stdarg.h>
// libc++ dep
#include <iostream>
#include <string>
// POSIX specific
extern "C" {
#include <unistd.h>
#include <errno.h>
#include <pthread.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <time.h>
}
#include "arch/macOS/timer.h"

348
librviz_tutorial/sdk/src/arch/macOS/net_serial.cpp Executable file
View File

@ -0,0 +1,348 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#include "arch/macOS/arch_macOS.h"
#include "arch/macOS/net_serial.h"
#include <termios.h>
#include <sys/select.h>
namespace rp{ namespace arch{ namespace net{
raw_serial::raw_serial()
: rp::hal::serial_rxtx()
, _baudrate(0)
, _flags(0)
, serial_fd(-1)
{
_init();
}
raw_serial::~raw_serial()
{
close();
}
bool raw_serial::open()
{
return open(_portName, _baudrate, _flags);
}
bool raw_serial::bind(const char * portname, uint32_t baudrate, uint32_t flags)
{
strncpy(_portName, portname, sizeof(_portName));
_baudrate = baudrate;
_flags = flags;
return true;
}
bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags)
{
if (isOpened()) close();
serial_fd = ::open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
if (serial_fd == -1) return false;
struct termios options, oldopt;
tcgetattr(serial_fd, &oldopt);
bzero(&options,sizeof(struct termios));
_u32 termbaud = getTermBaudBitmap(baudrate);
if (termbaud == (_u32)-1) {
fprintf(stderr, "Baudrate %d is not supported on macOS\r\n", baudrate);
close();
return false;
}
cfsetispeed(&options, termbaud);
cfsetospeed(&options, termbaud);
// enable rx and tx
options.c_cflag |= (CLOCAL | CREAD);
options.c_cflag &= ~PARENB; //no checkbit
options.c_cflag &= ~CSTOPB; //1bit stop bit
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8; /* Select 8 data bits */
#ifdef CNEW_RTSCTS
options.c_cflag &= ~CNEW_RTSCTS; // no hw flow control
#endif
options.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control
// raw input mode
options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
// raw output mode
options.c_oflag &= ~OPOST;
tcflush(serial_fd,TCIFLUSH);
/*
if (fcntl(serial_fd, F_SETFL, FNDELAY))
{
close();
return false;
}
*/
if (tcsetattr(serial_fd, TCSANOW, &options))
{
close();
return false;
}
_is_serial_opened = true;
//Clear the DTR bit to let the motor spin
clearDTR();
return true;
}
void raw_serial::close()
{
if (serial_fd != -1)
::close(serial_fd);
serial_fd = -1;
_is_serial_opened = false;
}
int raw_serial::senddata(const unsigned char * data, _word_size_t size)
{
// FIXME: non-block io should be used
if (!isOpened()) return 0;
if (data == NULL || size ==0) return 0;
size_t tx_len = 0;
required_tx_cnt = 0;
do {
int ans = ::write(serial_fd, data + tx_len, size-tx_len);
if (ans == -1) return tx_len;
tx_len += ans;
required_tx_cnt = tx_len;
}while (tx_len<size);
return tx_len;
}
int raw_serial::recvdata(unsigned char * data, _word_size_t size)
{
if (!isOpened()) return 0;
int ans = ::read(serial_fd, data, size);
if (ans == -1) ans=0;
required_rx_cnt = ans;
return ans;
}
void raw_serial::flush( _u32 flags)
{
tcflush(serial_fd,TCIFLUSH);
}
int raw_serial::waitforsent(_u32 timeout, _word_size_t * returned_size)
{
if (returned_size) *returned_size = required_tx_cnt;
return 0;
}
int raw_serial::waitforrecv(_u32 timeout, _word_size_t * returned_size)
{
if (!isOpened() ) return -1;
if (returned_size) *returned_size = required_rx_cnt;
return 0;
}
int raw_serial::waitfordata(_word_size_t data_count, _u32 timeout, _word_size_t * returned_size)
{
_word_size_t length = 0;
if (returned_size==NULL) returned_size=(_word_size_t *)&length;
*returned_size = 0;
int max_fd;
fd_set input_set;
struct timeval timeout_val;
/* Initialize the input set */
FD_ZERO(&input_set);
FD_SET(serial_fd, &input_set);
max_fd = serial_fd + 1;
/* Initialize the timeout structure */
timeout_val.tv_sec = timeout / 1000;
timeout_val.tv_usec = (timeout % 1000) * 1000;
if ( isOpened() )
{
if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR;
if (*returned_size >= data_count)
{
return 0;
}
}
while ( isOpened() )
{
/* Do the select */
int n = ::select(max_fd, &input_set, NULL, NULL, &timeout_val);
if (n < 0)
{
// select error
return ANS_DEV_ERR;
}
else if (n == 0)
{
// time out
return ANS_TIMEOUT;
}
else
{
// data avaliable
assert (FD_ISSET(serial_fd, &input_set));
if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR;
if (*returned_size >= data_count)
{
return 0;
}
else
{
int remain_timeout = timeout_val.tv_sec*1000000 + timeout_val.tv_usec;
int expect_remain_time = (data_count - *returned_size)*1000000*8/_baudrate;
if (remain_timeout > expect_remain_time)
usleep(expect_remain_time);
}
}
}
return ANS_DEV_ERR;
}
size_t raw_serial::rxqueue_count()
{
if ( !isOpened() ) return 0;
size_t remaining;
if (::ioctl(serial_fd, FIONREAD, &remaining) == -1) return 0;
return remaining;
}
void raw_serial::setDTR()
{
if ( !isOpened() ) return;
uint32_t dtr_bit = TIOCM_DTR;
ioctl(serial_fd, TIOCMBIS, &dtr_bit);
}
void raw_serial::clearDTR()
{
if ( !isOpened() ) return;
uint32_t dtr_bit = TIOCM_DTR;
ioctl(serial_fd, TIOCMBIC, &dtr_bit);
}
void raw_serial::_init()
{
serial_fd = 0;
_portName[0] = 0;
required_tx_cnt = required_rx_cnt = 0;
}
_u32 raw_serial::getTermBaudBitmap(_u32 baud)
{
#define BAUD_CONV(_baud_) case _baud_: return _baud_
switch (baud)
{
BAUD_CONV(1200);
BAUD_CONV(1800);
BAUD_CONV(2400);
BAUD_CONV(4800);
BAUD_CONV(9600);
BAUD_CONV(19200);
BAUD_CONV(38400);
BAUD_CONV(57600);
BAUD_CONV(115200);
BAUD_CONV(230400);
BAUD_CONV(460800);
BAUD_CONV(500000);
BAUD_CONV(576000);
BAUD_CONV(921600);
BAUD_CONV(1000000);
BAUD_CONV(1152000);
BAUD_CONV(1500000);
BAUD_CONV(2000000);
BAUD_CONV(2500000);
BAUD_CONV(3000000);
BAUD_CONV(3500000);
BAUD_CONV(4000000);
}
return -1;
}
}}} //end rp::arch::net
//begin rp::hal
namespace rp{ namespace hal{
serial_rxtx * serial_rxtx::CreateRxTx()
{
return new rp::arch::net::raw_serial();
}
void serial_rxtx::ReleaseRxTx(serial_rxtx *rxtx)
{
delete rxtx;
}
}} //end rp::hal

84
librviz_tutorial/sdk/src/arch/macOS/net_serial.h Executable file
View File

@ -0,0 +1,84 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
#include "hal/abs_rxtx.h"
namespace rp{ namespace arch{ namespace net{
class raw_serial : public rp::hal::serial_rxtx
{
public:
enum{
SERIAL_RX_BUFFER_SIZE = 512,
SERIAL_TX_BUFFER_SIZE = 128,
};
raw_serial();
virtual ~raw_serial();
virtual bool bind(const char * portname, uint32_t baudrate, uint32_t flags = 0);
virtual bool open();
virtual void close();
virtual void flush( _u32 flags);
virtual int waitfordata(_word_size_t data_count,_u32 timeout = -1, _word_size_t * returned_size = NULL);
virtual int senddata(const unsigned char * data, _word_size_t size);
virtual int recvdata(unsigned char * data, _word_size_t size);
virtual int waitforsent(_u32 timeout = -1, _word_size_t * returned_size = NULL);
virtual int waitforrecv(_u32 timeout = -1, _word_size_t * returned_size = NULL);
virtual size_t rxqueue_count();
virtual void setDTR();
virtual void clearDTR();
_u32 getTermBaudBitmap(_u32 baud);
protected:
bool open(const char * portname, uint32_t baudrate, uint32_t flags = 0);
void _init();
char _portName[200];
uint32_t _baudrate;
uint32_t _flags;
int serial_fd;
size_t required_tx_cnt;
size_t required_rx_cnt;
};
}}}

858
librviz_tutorial/sdk/src/arch/macOS/net_socket.cpp Executable file
View File

@ -0,0 +1,858 @@
/*
* RoboPeak Project
* HAL Layer - Socket Interface
* Copyright 2018 RoboPeak Project
*
* macOS Implementation
*/
#include "sdkcommon.h"
#include "../../hal/socket.h"
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <arpa/inet.h>
#include <netdb.h>
namespace rp{ namespace net {
static inline int _halAddrTypeToOSType(SocketAddress::address_type_t type)
{
switch (type) {
case SocketAddress::ADDRESS_TYPE_INET:
return AF_INET;
case SocketAddress::ADDRESS_TYPE_INET6:
return AF_INET6;
case SocketAddress::ADDRESS_TYPE_UNSPEC:
return AF_UNSPEC;
default:
assert(!"should not reach here");
return AF_UNSPEC;
}
}
SocketAddress::SocketAddress()
{
_platform_data = reinterpret_cast<void *>(new sockaddr_storage);
memset(_platform_data, 0, sizeof(sockaddr_storage));
reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
}
SocketAddress::SocketAddress(const SocketAddress & src)
{
_platform_data = reinterpret_cast<void *>(new sockaddr_storage);
memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage));
}
SocketAddress::SocketAddress(const char * addrString, int port, SocketAddress::address_type_t type)
{
_platform_data = reinterpret_cast<void *>(new sockaddr_storage);
memset(_platform_data, 0, sizeof(sockaddr_storage));
// default to ipv4 in case the following operation fails
reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
setAddressFromString(addrString, type);
setPort(port);
}
SocketAddress::SocketAddress(void * platform_data)
: _platform_data(platform_data)
{}
SocketAddress & SocketAddress::operator = (const SocketAddress &src)
{
memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage));
return *this;
}
SocketAddress::~SocketAddress()
{
delete reinterpret_cast<sockaddr_storage *>(_platform_data);
}
SocketAddress::address_type_t SocketAddress::getAddressType() const
{
switch(reinterpret_cast<const sockaddr_storage *>(_platform_data)->ss_family) {
case AF_INET:
return ADDRESS_TYPE_INET;
case AF_INET6:
return ADDRESS_TYPE_INET6;
default:
assert(!"should not reach here");
return ADDRESS_TYPE_INET;
}
}
int SocketAddress::getPort() const
{
switch (getAddressType()) {
case ADDRESS_TYPE_INET:
return (int)ntohs(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_port);
case ADDRESS_TYPE_INET6:
return (int)ntohs(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_port);
default:
return 0;
}
}
u_result SocketAddress::setPort(int port)
{
switch (getAddressType()) {
case ADDRESS_TYPE_INET:
reinterpret_cast<sockaddr_in *>(_platform_data)->sin_port = htons((short)port);
break;
case ADDRESS_TYPE_INET6:
reinterpret_cast<sockaddr_in6 *>(_platform_data)->sin6_port = htons((short)port);
break;
default:
return RESULT_OPERATION_FAIL;
}
return RESULT_OK;
}
u_result SocketAddress::setAddressFromString(const char * address_string, SocketAddress::address_type_t type)
{
int ans = 0;
int prevPort = getPort();
switch (type) {
case ADDRESS_TYPE_INET:
reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
ans = inet_pton(AF_INET,
address_string,
&reinterpret_cast<sockaddr_in *>(_platform_data)->sin_addr);
break;
case ADDRESS_TYPE_INET6:
reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET6;
ans = inet_pton(AF_INET6,
address_string,
&reinterpret_cast<sockaddr_in6 *>(_platform_data)->sin6_addr);
break;
default:
return RESULT_INVALID_DATA;
}
setPort(prevPort);
return ans<=0?RESULT_INVALID_DATA:RESULT_OK;
}
u_result SocketAddress::getAddressAsString(char * buffer, size_t buffersize) const
{
int net_family = reinterpret_cast<const sockaddr_storage *>(_platform_data)->ss_family;
const char *ans = NULL;
switch (net_family) {
case AF_INET:
ans = inet_ntop(net_family, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr,
buffer, buffersize);
break;
case AF_INET6:
ans = inet_ntop(net_family, &reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr,
buffer, buffersize);
break;
}
return !ans?RESULT_OPERATION_FAIL:RESULT_OK;
}
size_t SocketAddress::LoopUpHostName(const char * hostname, const char * sevicename, std::vector<SocketAddress> &addresspool , bool performDNS, SocketAddress::address_type_t type)
{
struct addrinfo hints;
struct addrinfo *result;
int ans;
memset(&hints, 0, sizeof(struct addrinfo));
hints.ai_family = _halAddrTypeToOSType(type);
hints.ai_flags = AI_PASSIVE;
if (!performDNS) {
hints.ai_family |= AI_NUMERICSERV | AI_NUMERICHOST;
}
ans = getaddrinfo(hostname, sevicename, &hints, &result);
addresspool.clear();
if (ans != 0) {
// hostname loopup failed
return 0;
}
for (struct addrinfo * cursor = result; cursor != NULL; cursor = cursor->ai_next) {
if (cursor->ai_family == ADDRESS_TYPE_INET || cursor->ai_family == ADDRESS_TYPE_INET6) {
sockaddr_storage * storagebuffer = new sockaddr_storage;
assert(sizeof(sockaddr_storage) >= cursor->ai_addrlen);
memcpy(storagebuffer, cursor->ai_addr, cursor->ai_addrlen);
addresspool.push_back(SocketAddress(storagebuffer));
}
}
freeaddrinfo(result);
return addresspool.size();
}
u_result SocketAddress::getRawAddress(_u8 * buffer, size_t bufferSize) const
{
switch (getAddressType()) {
case ADDRESS_TYPE_INET:
if (bufferSize < sizeof(in_addr_t)) return RESULT_INSUFFICIENT_MEMORY;
memcpy(buffer, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr, sizeof(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr));
break;
case ADDRESS_TYPE_INET6:
if (bufferSize < sizeof(in6_addr)) return RESULT_INSUFFICIENT_MEMORY;
memcpy(buffer, reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr, sizeof(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr));
break;
default:
return RESULT_OPERATION_FAIL;
}
return RESULT_OK;
}
void SocketAddress::setLoopbackAddress(SocketAddress::address_type_t type)
{
int prevPort = getPort();
switch (type) {
case ADDRESS_TYPE_INET:
{
sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
addrv4->sin_family = AF_INET;
addrv4->sin_addr.s_addr = htonl(INADDR_LOOPBACK);
}
break;
case ADDRESS_TYPE_INET6:
{
sockaddr_in6 * addrv6 = reinterpret_cast<sockaddr_in6 *>(_platform_data);
addrv6->sin6_family = AF_INET6;
addrv6->sin6_addr = in6addr_loopback;
}
break;
default:
return;
}
setPort(prevPort);
}
void SocketAddress::setBroadcastAddressIPv4()
{
int prevPort = getPort();
sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
addrv4->sin_family = AF_INET;
addrv4->sin_addr.s_addr = htonl(INADDR_BROADCAST);
setPort(prevPort);
}
void SocketAddress::setAnyAddress(SocketAddress::address_type_t type)
{
int prevPort = getPort();
switch (type) {
case ADDRESS_TYPE_INET:
{
sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
addrv4->sin_family = AF_INET;
addrv4->sin_addr.s_addr = htonl(INADDR_ANY);
}
break;
case ADDRESS_TYPE_INET6:
{
sockaddr_in6 * addrv6 = reinterpret_cast<sockaddr_in6 *>(_platform_data);
addrv6->sin6_family = AF_INET6;
addrv6->sin6_addr = in6addr_any;
}
break;
default:
return;
}
setPort(prevPort);
}
}}
///--------------------------------
namespace rp { namespace arch { namespace net{
using namespace rp::net;
class _single_thread StreamSocketImpl : public StreamSocket
{
public:
StreamSocketImpl(int fd)
: _socket_fd(fd)
{
assert(fd>=0);
int bool_true = 1;
::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR , (char *)&bool_true, sizeof(bool_true) );
::setsockopt( _socket_fd, SOL_SOCKET, SO_NOSIGPIPE, (char*)&bool_true, sizeof(bool_true));
enableNoDelay(true);
this->setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH);
}
virtual ~StreamSocketImpl()
{
close(_socket_fd);
}
virtual void dispose()
{
delete this;
}
virtual u_result bind(const SocketAddress & localaddr)
{
const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(localaddr.getPlatformData());
assert(addr);
int ans = ::bind(_socket_fd, addr, sizeof(sockaddr_storage));
if (ans) {
return RESULT_OPERATION_FAIL;
} else {
return RESULT_OK;
}
}
virtual u_result getLocalAddress(SocketAddress & localaddr)
{
struct sockaddr * addr = reinterpret_cast<struct sockaddr *>( const_cast<void *>(localaddr.getPlatformData())); //donnot do this at home...
assert(addr);
size_t actualsize = sizeof(sockaddr_storage);
int ans = ::getsockname(_socket_fd, addr, (socklen_t*)&actualsize);
assert(actualsize <= sizeof(sockaddr_storage));
assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
return ans?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk)
{
int ans;
timeval tv;
tv.tv_sec = timeout / 1000;
tv.tv_usec = (timeout % 1000) * 1000;
if (msk & SOCKET_DIR_RD) {
ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) );
if (ans) return RESULT_OPERATION_FAIL;
}
if (msk & SOCKET_DIR_WR) {
ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv) );
if (ans) return RESULT_OPERATION_FAIL;
}
return RESULT_OK;
}
virtual u_result connect(const SocketAddress & pairAddress)
{
const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(pairAddress.getPlatformData());
int ans = ::connect(_socket_fd, addr, sizeof(sockaddr_storage));
if (!ans) return RESULT_OK;
switch (errno) {
case EAFNOSUPPORT:
return RESULT_OPERATION_NOT_SUPPORT;
#if 0
case EINPROGRESS:
return RESULT_OK; //treat async connection as good status
#endif
case ETIMEDOUT:
return RESULT_OPERATION_TIMEOUT;
default:
return RESULT_OPERATION_FAIL;
}
}
virtual u_result listen(int backlog)
{
int ans = ::listen( _socket_fd, backlog);
return ans?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual StreamSocket * accept(SocketAddress * pairAddress)
{
size_t addrsize;
addrsize = sizeof(sockaddr_storage);
int pair_socket = ::accept( _socket_fd, pairAddress?reinterpret_cast<struct sockaddr *>(const_cast<void *>(pairAddress->getPlatformData())):NULL
, (socklen_t*)&addrsize);
if (pair_socket>=0) {
return new StreamSocketImpl(pair_socket);
} else {
return NULL;
}
}
virtual u_result waitforIncomingConnection(_u32 timeout)
{
return waitforData(timeout);
}
virtual u_result send(const void * buffer, size_t len)
{
size_t ans = ::send( _socket_fd, buffer, len, 0);
if (ans == (int)len) {
return RESULT_OK;
} else {
switch (errno) {
case EAGAIN:
#if EWOULDBLOCK!=EAGAIN
case EWOULDBLOCK:
#endif
return RESULT_OPERATION_TIMEOUT;
default:
return RESULT_OPERATION_FAIL;
}
}
}
virtual u_result recv(void *buf, size_t len, size_t & recv_len)
{
size_t ans = ::recv( _socket_fd, buf, len, 0);
if (ans == (size_t)-1) {
recv_len = 0;
switch (errno) {
case EAGAIN:
#if EWOULDBLOCK!=EAGAIN
case EWOULDBLOCK:
#endif
return RESULT_OPERATION_TIMEOUT;
default:
return RESULT_OPERATION_FAIL;
}
} else {
recv_len = ans;
return RESULT_OK;
}
}
#if 0
virtual u_result recvNoWait(void *buf, size_t len, size_t & recv_len)
{
size_t ans = ::recv( _socket_fd, buf, len, MSG_DONTWAIT);
if (ans == (size_t)-1) {
recv_len = 0;
if (errno == EAGAIN || errno == EWOULDBLOCK) {
return RESULT_OK;
} else {
return RESULT_OPERATION_FAIL;
}
} else {
recv_len = ans;
return RESULT_OK;
}
}
#endif
virtual u_result getPeerAddress(SocketAddress & peerAddr)
{
struct sockaddr * addr = reinterpret_cast<struct sockaddr *>(const_cast<void *>(peerAddr.getPlatformData())); //donnot do this at home...
assert(addr);
size_t actualsize = sizeof(sockaddr_storage);
int ans = ::getpeername(_socket_fd, addr, (socklen_t*)&actualsize);
assert(actualsize <= sizeof(sockaddr_storage));
assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
return ans?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual u_result shutdown(socket_direction_mask mask)
{
int shutdw_opt ;
switch (mask) {
case SOCKET_DIR_RD:
shutdw_opt = SHUT_RD;
break;
case SOCKET_DIR_WR:
shutdw_opt = SHUT_WR;
break;
case SOCKET_DIR_BOTH:
default:
shutdw_opt = SHUT_RDWR;
}
int ans = ::shutdown(_socket_fd, shutdw_opt);
return ans?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual u_result enableKeepAlive(bool enable)
{
int bool_true = enable?1:0;
return ::setsockopt( _socket_fd, SOL_SOCKET, SO_KEEPALIVE , &bool_true, sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual u_result enableNoDelay(bool enable )
{
int bool_true = enable?1:0;
return ::setsockopt( _socket_fd, IPPROTO_TCP, TCP_NODELAY,&bool_true, sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual u_result waitforSent(_u32 timeout )
{
fd_set wrset;
FD_ZERO(&wrset);
FD_SET(_socket_fd, &wrset);
timeval tv;
tv.tv_sec = timeout / 1000;
tv.tv_usec = (timeout % 1000) * 1000;
int ans = ::select(_socket_fd+1, NULL, &wrset, NULL, &tv);
switch (ans) {
case 1:
// fired
return RESULT_OK;
case 0:
// timeout
return RESULT_OPERATION_TIMEOUT;
default:
delay(0); //relax cpu
return RESULT_OPERATION_FAIL;
}
}
virtual u_result waitforData(_u32 timeout )
{
fd_set rdset;
FD_ZERO(&rdset);
FD_SET(_socket_fd, &rdset);
timeval tv;
tv.tv_sec = timeout / 1000;
tv.tv_usec = (timeout % 1000) * 1000;
int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv);
switch (ans) {
case 1:
// fired
return RESULT_OK;
case 0:
// timeout
return RESULT_OPERATION_TIMEOUT;
default:
delay(0); //relax cpu
return RESULT_OPERATION_FAIL;
}
}
protected:
int _socket_fd;
};
class _single_thread DGramSocketImpl : public DGramSocket
{
public:
DGramSocketImpl(int fd)
: _socket_fd(fd)
{
assert(fd>=0);
int bool_true = 1;
::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR | SO_BROADCAST , (char *)&bool_true, sizeof(bool_true) );
setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH);
}
virtual ~DGramSocketImpl()
{
close(_socket_fd);
}
virtual void dispose()
{
delete this;
}
virtual u_result bind(const SocketAddress & localaddr)
{
const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(localaddr.getPlatformData());
assert(addr);
int ans = ::bind(_socket_fd, addr, sizeof(sockaddr_storage));
if (ans) {
return RESULT_OPERATION_FAIL;
} else {
return RESULT_OK;
}
}
virtual u_result getLocalAddress(SocketAddress & localaddr)
{
struct sockaddr * addr = reinterpret_cast<struct sockaddr *>(const_cast<void *>((localaddr.getPlatformData()))); //donnot do this at home...
assert(addr);
size_t actualsize = sizeof(sockaddr_storage);
int ans = ::getsockname(_socket_fd, addr, (socklen_t*)&actualsize);
assert(actualsize <= sizeof(sockaddr_storage));
assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
return ans?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk)
{
int ans;
timeval tv;
tv.tv_sec = timeout / 1000;
tv.tv_usec = (timeout % 1000) * 1000;
if (msk & SOCKET_DIR_RD) {
ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) );
if (ans) return RESULT_OPERATION_FAIL;
}
if (msk & SOCKET_DIR_WR) {
ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv) );
if (ans) return RESULT_OPERATION_FAIL;
}
return RESULT_OK;
}
virtual u_result waitforSent(_u32 timeout )
{
fd_set wrset;
FD_ZERO(&wrset);
FD_SET(_socket_fd, &wrset);
timeval tv;
tv.tv_sec = timeout / 1000;
tv.tv_usec = (timeout % 1000) * 1000;
int ans = ::select(_socket_fd+1, NULL, &wrset, NULL, &tv);
switch (ans) {
case 1:
// fired
return RESULT_OK;
case 0:
// timeout
return RESULT_OPERATION_TIMEOUT;
default:
delay(0); //relax cpu
return RESULT_OPERATION_FAIL;
}
}
virtual u_result waitforData(_u32 timeout )
{
fd_set rdset;
FD_ZERO(&rdset);
FD_SET(_socket_fd, &rdset);
timeval tv;
tv.tv_sec = timeout / 1000;
tv.tv_usec = (timeout % 1000) * 1000;
int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv);
switch (ans) {
case 1:
// fired
return RESULT_OK;
case 0:
// timeout
return RESULT_OPERATION_TIMEOUT;
default:
delay(0); //relax cpu
return RESULT_OPERATION_FAIL;
}
}
virtual u_result sendTo(const SocketAddress & target, const void * buffer, size_t len)
{
const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(target.getPlatformData());
assert(addr);
size_t ans = ::sendto( _socket_fd, buffer, len, 0, addr, sizeof(sockaddr_storage));
if (ans != (size_t)-1) {
assert(ans == (int)len);
return RESULT_OK;
} else {
switch (errno) {
case EAGAIN:
#if EWOULDBLOCK!=EAGAIN
case EWOULDBLOCK:
#endif
return RESULT_OPERATION_TIMEOUT;
case EMSGSIZE:
return RESULT_INVALID_DATA;
default:
return RESULT_OPERATION_FAIL;
}
}
}
virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr)
{
struct sockaddr * addr = (sourceAddr?reinterpret_cast<struct sockaddr *>(const_cast<void *>(sourceAddr->getPlatformData())):NULL);
size_t source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0);
size_t ans = ::recvfrom( _socket_fd, buf, len, 0, addr, (socklen_t*)&source_addr_size);
if (ans == (size_t)-1) {
recv_len = 0;
switch (errno) {
case EAGAIN:
#if EWOULDBLOCK!=EAGAIN
case EWOULDBLOCK:
#endif
return RESULT_OPERATION_TIMEOUT;
default:
return RESULT_OPERATION_FAIL;
}
} else {
recv_len = ans;
return RESULT_OK;
}
}
#if 0
virtual u_result recvFromNoWait(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr)
{
struct sockaddr * addr = (sourceAddr?reinterpret_cast<struct sockaddr *>(const_cast<void *>(sourceAddr->getPlatformData())):NULL);
size_t source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0);
size_t ans = ::recvfrom( _socket_fd, buf, len, MSG_DONTWAIT, addr, &source_addr_size);
if (ans == (size_t)-1) {
recv_len = 0;
if (errno == EAGAIN || errno == EWOULDBLOCK) {
return RESULT_OK;
} else {
return RESULT_OPERATION_FAIL;
}
} else {
recv_len = ans;
return RESULT_OK;
}
}
#endif
protected:
int _socket_fd;
};
}}}
namespace rp { namespace net{
static inline int _socketHalFamilyToOSFamily(SocketBase::socket_family_t family)
{
switch (family) {
case SocketBase::SOCKET_FAMILY_INET:
return AF_INET;
case SocketBase::SOCKET_FAMILY_INET6:
return AF_INET6;
case SocketBase::SOCKET_FAMILY_RAW:
assert(!"should not reach here, AF_PACKET is not supported on macOS");
return AF_INET;
default:
assert(!"should not reach here");
return AF_INET; // force treating as IPv4 in release mode
}
}
StreamSocket * StreamSocket::CreateSocket(SocketBase::socket_family_t family)
{
if (family == SOCKET_FAMILY_RAW) return NULL;
int socket_family = _socketHalFamilyToOSFamily(family);
int socket_fd = ::socket(socket_family, SOCK_STREAM, 0);
if (socket_fd == -1) return NULL;
StreamSocket * newborn = static_cast<StreamSocket *>(new rp::arch::net::StreamSocketImpl(socket_fd));
return newborn;
}
DGramSocket * DGramSocket::CreateSocket(SocketBase::socket_family_t family)
{
int socket_family = _socketHalFamilyToOSFamily(family);
int socket_fd = ::socket(socket_family, (family==SOCKET_FAMILY_RAW)?SOCK_RAW:SOCK_DGRAM, 0);
if (socket_fd == -1) return NULL;
DGramSocket * newborn = static_cast<DGramSocket *>(new rp::arch::net::DGramSocketImpl(socket_fd));
return newborn;
}
}}

79
librviz_tutorial/sdk/src/arch/macOS/thread.hpp Executable file
View File

@ -0,0 +1,79 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#include "arch/macOS/arch_macOS.h"
namespace rp{ namespace hal{
Thread Thread::create(thread_proc_t proc, void * data)
{
Thread newborn(proc, data);
// tricky code, we assume pthread_t is not a structure but a word size value
assert( sizeof(newborn._handle) >= sizeof(pthread_t));
pthread_create((pthread_t *)&newborn._handle, NULL,(void * (*)(void *))proc, data);
return newborn;
}
u_result Thread::terminate()
{
if (!this->_handle) return RESULT_OK;
// return pthread_cancel((pthread_t)this->_handle)==0?RESULT_OK:RESULT_OPERATION_FAIL;
return RESULT_OK;
}
u_result Thread::setPriority( priority_val_t p)
{
if (!this->_handle) return RESULT_OPERATION_FAIL;
// simply ignore this request
return RESULT_OK;
}
Thread::priority_val_t Thread::getPriority()
{
return PRIORITY_NORMAL;
}
u_result Thread::join(unsigned long timeout)
{
if (!this->_handle) return RESULT_OK;
pthread_join((pthread_t)(this->_handle), NULL);
return RESULT_OK;
}
}}

53
librviz_tutorial/sdk/src/arch/macOS/timer.cpp Executable file
View File

@ -0,0 +1,53 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#include "arch/macOS/arch_macOS.h"
namespace rp{ namespace arch{
_u64 getus()
{
timeval now;
gettimeofday(&now,NULL);
return now.tv_sec*1000000 + now.tv_usec;
}
_u32 rp_getms()
{
timeval now;
gettimeofday(&now,NULL);
return now.tv_sec*1000L + now.tv_usec/1000L;
}
}}

57
librviz_tutorial/sdk/src/arch/macOS/timer.h Executable file
View File

@ -0,0 +1,57 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
#include "rptypes.h"
#include <unistd.h>
static inline void delay(_word_size_t ms){
while (ms>=1000){
usleep(1000*1000);
ms-=1000;
};
if (ms!=0)
usleep(ms*1000);
}
// TODO: the highest timer interface should be clock_gettime
namespace rp{ namespace arch{
_u64 rp_getus();
_u32 rp_getms();
}}
#define getms() rp::arch::rp_getms()

66
librviz_tutorial/sdk/src/arch/win32/arch_win32.h Executable file
View File

@ -0,0 +1,66 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
#pragma warning (disable: 4996)
#define _CRT_SECURE_NO_WARNINGS
#ifndef WINVER
#define WINVER 0x0500
#endif
#ifndef _WIN32_WINNT
#define _WIN32_WINNT 0x0501
#endif
#ifndef _WIN32_IE
#define _WIN32_IE 0x0501
#endif
#ifndef _RICHEDIT_VER
#define _RICHEDIT_VER 0x0200
#endif
#include <stddef.h>
#include <stdio.h>
#include <windows.h>
#include <stdlib.h> //for memcpy etc..
#include <process.h>
#include <direct.h>
#include "timer.h"

358
librviz_tutorial/sdk/src/arch/win32/net_serial.cpp Executable file
View File

@ -0,0 +1,358 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#include "sdkcommon.h"
#include "net_serial.h"
namespace rp{ namespace arch{ namespace net{
raw_serial::raw_serial()
: rp::hal::serial_rxtx()
, _serial_handle(NULL)
, _baudrate(0)
, _flags(0)
{
_init();
}
raw_serial::~raw_serial()
{
close();
CloseHandle(_ro.hEvent);
CloseHandle(_wo.hEvent);
CloseHandle(_wait_o.hEvent);
}
bool raw_serial::open()
{
return open(_portName, _baudrate, _flags);
}
bool raw_serial::bind(const char * portname, _u32 baudrate, _u32 flags)
{
strncpy(_portName, portname, sizeof(_portName));
_baudrate = baudrate;
_flags = flags;
return true;
}
bool raw_serial::open(const char * portname, _u32 baudrate, _u32 flags)
{
if (isOpened()) close();
_serial_handle = CreateFile(
portname,
GENERIC_READ | GENERIC_WRITE,
0,
NULL,
OPEN_EXISTING,
FILE_ATTRIBUTE_NORMAL | FILE_FLAG_OVERLAPPED,
NULL
);
if (_serial_handle == INVALID_HANDLE_VALUE) return false;
if (!SetupComm(_serial_handle, SERIAL_RX_BUFFER_SIZE, SERIAL_TX_BUFFER_SIZE))
{
close();
return false;
}
_dcb.BaudRate = baudrate;
_dcb.ByteSize = 8;
_dcb.Parity = NOPARITY;
_dcb.StopBits = ONESTOPBIT;
_dcb.fDtrControl = DTR_CONTROL_ENABLE;
if (!SetCommState(_serial_handle, &_dcb))
{
close();
return false;
}
if (!SetCommTimeouts(_serial_handle, &_co))
{
close();
return false;
}
if (!SetCommMask(_serial_handle, EV_RXCHAR | EV_ERR ))
{
close();
return false;
}
if (!PurgeComm(_serial_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR ))
{
close();
return false;
}
Sleep(30);
_is_serial_opened = true;
//Clear the DTR bit set DTR=high
clearDTR();
return true;
}
void raw_serial::close()
{
SetCommMask(_serial_handle, 0);
ResetEvent(_wait_o.hEvent);
CloseHandle(_serial_handle);
_serial_handle = INVALID_HANDLE_VALUE;
_is_serial_opened = false;
}
int raw_serial::senddata(const unsigned char * data, size_t size)
{
DWORD error;
DWORD w_len = 0, o_len = -1;
if (!isOpened()) return ANS_DEV_ERR;
if (data == NULL || size ==0) return 0;
if(ClearCommError(_serial_handle, &error, NULL) && error > 0)
PurgeComm(_serial_handle, PURGE_TXABORT | PURGE_TXCLEAR);
if(!WriteFile(_serial_handle, data, size, &w_len, &_wo))
if(GetLastError() != ERROR_IO_PENDING)
w_len = ANS_DEV_ERR;
return w_len;
}
int raw_serial::recvdata(unsigned char * data, size_t size)
{
if (!isOpened()) return 0;
DWORD r_len = 0;
if(!ReadFile(_serial_handle, data, size, &r_len, &_ro))
{
if(GetLastError() == ERROR_IO_PENDING)
{
if(!GetOverlappedResult(_serial_handle, &_ro, &r_len, FALSE))
{
if(GetLastError() != ERROR_IO_INCOMPLETE)
r_len = 0;
}
}
else
r_len = 0;
}
return r_len;
}
void raw_serial::flush( _u32 flags)
{
PurgeComm(_serial_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR );
}
int raw_serial::waitforsent(_u32 timeout, size_t * returned_size)
{
if (!isOpened() ) return ANS_DEV_ERR;
DWORD w_len = 0;
_word_size_t ans =0;
if (WaitForSingleObject(_wo.hEvent, timeout) == WAIT_TIMEOUT)
{
ans = ANS_TIMEOUT;
goto _final;
}
if(!GetOverlappedResult(_serial_handle, &_wo, &w_len, FALSE))
{
ans = ANS_DEV_ERR;
}
_final:
if (returned_size) *returned_size = w_len;
return ans;
}
int raw_serial::waitforrecv(_u32 timeout, size_t * returned_size)
{
if (!isOpened() ) return -1;
DWORD r_len = 0;
_word_size_t ans =0;
if (WaitForSingleObject(_ro.hEvent, timeout) == WAIT_TIMEOUT)
{
ans = ANS_TIMEOUT;
}
if(!GetOverlappedResult(_serial_handle, &_ro, &r_len, FALSE))
{
ans = ANS_DEV_ERR;
}
if (returned_size) *returned_size = r_len;
return ans;
}
int raw_serial::waitfordata(size_t data_count, _u32 timeout, size_t * returned_size)
{
COMSTAT stat;
DWORD error;
DWORD msk,length;
size_t dummy_length;
if (returned_size==NULL) returned_size=(size_t *)&dummy_length;
if ( isOpened()) {
size_t rxqueue_remaining = rxqueue_count();
if (rxqueue_remaining >= data_count) {
*returned_size = rxqueue_remaining;
return 0;
}
}
while ( isOpened() )
{
msk = 0;
SetCommMask(_serial_handle, EV_RXCHAR | EV_ERR );
if(!WaitCommEvent(_serial_handle, &msk, &_wait_o))
{
if(GetLastError() == ERROR_IO_PENDING)
{
if (WaitForSingleObject(_wait_o.hEvent, timeout) == WAIT_TIMEOUT)
{
*returned_size =0;
return ANS_TIMEOUT;
}
GetOverlappedResult(_serial_handle, &_wait_o, &length, TRUE);
::ResetEvent(_wait_o.hEvent);
}else
{
ClearCommError(_serial_handle, &error, &stat);
*returned_size = stat.cbInQue;
return ANS_DEV_ERR;
}
}
if(msk & EV_ERR){
// FIXME: may cause problem here
ClearCommError(_serial_handle, &error, &stat);
}
if(msk & EV_RXCHAR){
ClearCommError(_serial_handle, &error, &stat);
if(stat.cbInQue >= data_count)
{
*returned_size = stat.cbInQue;
return 0;
}
}
}
*returned_size=0;
return ANS_DEV_ERR;
}
size_t raw_serial::rxqueue_count()
{
if ( !isOpened() ) return 0;
COMSTAT com_stat;
DWORD error;
DWORD r_len = 0;
if(ClearCommError(_serial_handle, &error, &com_stat) && error > 0)
{
PurgeComm(_serial_handle, PURGE_RXABORT | PURGE_RXCLEAR);
return 0;
}
return com_stat.cbInQue;
}
void raw_serial::setDTR()
{
if ( !isOpened() ) return;
EscapeCommFunction(_serial_handle, SETDTR);
}
void raw_serial::clearDTR()
{
if ( !isOpened() ) return;
EscapeCommFunction(_serial_handle, CLRDTR);
}
void raw_serial::_init()
{
memset(&_dcb, 0, sizeof(_dcb));
_dcb.DCBlength = sizeof(_dcb);
_serial_handle = INVALID_HANDLE_VALUE;
memset(&_co, 0, sizeof(_co));
_co.ReadIntervalTimeout = 0;
_co.ReadTotalTimeoutMultiplier = 0;
_co.ReadTotalTimeoutConstant = 0;
_co.WriteTotalTimeoutMultiplier = 0;
_co.WriteTotalTimeoutConstant = 0;
memset(&_ro, 0, sizeof(_ro));
memset(&_wo, 0, sizeof(_wo));
memset(&_wait_o, 0, sizeof(_wait_o));
_ro.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
_wo.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
_wait_o.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
_portName[0] = 0;
}
}}} //end rp::arch::net
//begin rp::hal
namespace rp{ namespace hal{
serial_rxtx * serial_rxtx::CreateRxTx()
{
return new rp::arch::net::raw_serial();
}
void serial_rxtx::ReleaseRxTx( serial_rxtx * rxtx)
{
delete rxtx;
}
}} //end rp::hal

86
librviz_tutorial/sdk/src/arch/win32/net_serial.h Executable file
View File

@ -0,0 +1,86 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
#include "hal/abs_rxtx.h"
namespace rp{ namespace arch{ namespace net{
class raw_serial : public rp::hal::serial_rxtx
{
public:
enum{
SERIAL_RX_BUFFER_SIZE = 512,
SERIAL_TX_BUFFER_SIZE = 128,
SERIAL_RX_TIMEOUT = 2000,
SERIAL_TX_TIMEOUT = 2000,
};
raw_serial();
virtual ~raw_serial();
virtual bool bind(const char * portname, _u32 baudrate, _u32 flags = 0);
virtual bool open();
virtual void close();
virtual void flush( _u32 flags);
virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL);
virtual int senddata(const unsigned char * data, size_t size);
virtual int recvdata(unsigned char * data, size_t size);
virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL);
virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL);
virtual size_t rxqueue_count();
virtual void setDTR();
virtual void clearDTR();
protected:
bool open(const char * portname, _u32 baudrate, _u32 flags);
void _init();
char _portName[20];
uint32_t _baudrate;
uint32_t _flags;
OVERLAPPED _ro, _wo;
OVERLAPPED _wait_o;
volatile HANDLE _serial_handle;
DCB _dcb;
COMMTIMEOUTS _co;
};
}}}

876
librviz_tutorial/sdk/src/arch/win32/net_socket.cpp Executable file
View File

@ -0,0 +1,876 @@
/*
* RoboPeak Project
* HAL Layer - Socket Interface
* Copyright 2009 - 2013 RoboPeak Project
*
* Win32 Implementation
*/
#define _WINSOCKAPI_
#include "sdkcommon.h"
#include "..\..\hal\socket.h"
#include <windows.h>
#include <winsock2.h>
#include <ws2tcpip.h>
#include <stdlib.h>
#include <stdio.h>
#pragma comment (lib, "Ws2_32.lib")
namespace rp{ namespace net {
static volatile bool _isWSAStartupCalled = false;
static inline bool _checkWSAStartup() {
int iResult;
WSADATA wsaData;
if (!_isWSAStartupCalled) {
iResult = WSAStartup(MAKEWORD(2,2), &wsaData);
if (iResult != 0) {
return false;
}
_isWSAStartupCalled = true;
}
return true;
}
static const char* _inet_ntop(int af, const void* src, char* dst, int cnt){
struct sockaddr_storage srcaddr;
memset(dst, 0, cnt);
memset(&srcaddr, 0, sizeof(struct sockaddr_storage));
srcaddr.ss_family = af;
switch (af) {
case AF_INET:
{
struct sockaddr_in * ipv4 = reinterpret_cast< struct sockaddr_in *>(&srcaddr);
memcpy(&(ipv4->sin_addr), src, sizeof(ipv4->sin_addr));
}
break;
case AF_INET6:
{
struct sockaddr_in6 * ipv6 = reinterpret_cast< struct sockaddr_in6 *>(&srcaddr);
memcpy(&(ipv6->sin6_addr), src, sizeof(ipv6->sin6_addr));
}
break;
}
if (WSAAddressToStringA((struct sockaddr*) &srcaddr, sizeof(struct sockaddr_storage), 0, dst, (LPDWORD) &cnt) != 0) {
DWORD rv = WSAGetLastError();
return NULL;
}
return dst;
}
static int _inet_pton(int Family, const char * pszAddrString, void* pAddrBuf)
{
struct sockaddr_storage tmpholder;
int actualSize = sizeof(sockaddr_storage);
int result = WSAStringToAddressA((char *)pszAddrString, Family, NULL, (sockaddr*)&tmpholder, &actualSize);
if (result) return -1;
switch (Family) {
case AF_INET:
{
struct sockaddr_in * ipv4 = reinterpret_cast< struct sockaddr_in *>(&tmpholder);
memcpy(pAddrBuf, &(ipv4->sin_addr), sizeof(ipv4->sin_addr));
}
break;
case AF_INET6:
{
struct sockaddr_in6 * ipv6 = reinterpret_cast< struct sockaddr_in6 *>(&tmpholder);
memcpy(pAddrBuf, &(ipv6->sin6_addr), sizeof(ipv6->sin6_addr));
}
break;
}
return 1;
}
static inline int _halAddrTypeToOSType(SocketAddress::address_type_t type)
{
switch (type) {
case SocketAddress::ADDRESS_TYPE_INET:
return AF_INET;
case SocketAddress::ADDRESS_TYPE_INET6:
return AF_INET6;
case SocketAddress::ADDRESS_TYPE_UNSPEC:
return AF_UNSPEC;
default:
assert(!"should not reach here");
return AF_UNSPEC;
}
}
SocketAddress::SocketAddress()
{
_checkWSAStartup();
_platform_data = reinterpret_cast<void *>(new sockaddr_storage);
memset(_platform_data, 0, sizeof(sockaddr_storage));
reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
}
SocketAddress::SocketAddress(const SocketAddress & src)
{
_platform_data = reinterpret_cast<void *>(new sockaddr_storage);
memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage));
}
SocketAddress::SocketAddress(const char * addrString, int port, SocketAddress::address_type_t type)
{
_checkWSAStartup();
_platform_data = reinterpret_cast<void *>(new sockaddr_storage);
memset(_platform_data, 0, sizeof(sockaddr_storage));
// default to ipv4 in case the following operation fails
reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
setAddressFromString(addrString, type);
setPort(port);
}
SocketAddress::SocketAddress(void * platform_data)
: _platform_data(platform_data)
{ _checkWSAStartup(); }
SocketAddress & SocketAddress::operator = (const SocketAddress &src)
{
memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage));
return *this;
}
SocketAddress::~SocketAddress()
{
delete reinterpret_cast<sockaddr_storage *>(_platform_data);
}
SocketAddress::address_type_t SocketAddress::getAddressType() const
{
switch(reinterpret_cast<const sockaddr_storage *>(_platform_data)->ss_family) {
case AF_INET:
return ADDRESS_TYPE_INET;
case AF_INET6:
return ADDRESS_TYPE_INET6;
default:
assert(!"should not reach here");
return ADDRESS_TYPE_INET;
}
}
int SocketAddress::getPort() const
{
switch (getAddressType()) {
case ADDRESS_TYPE_INET:
return (int)ntohs(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_port);
case ADDRESS_TYPE_INET6:
return (int)ntohs(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_port);
default:
return 0;
}
}
u_result SocketAddress::setPort(int port)
{
switch (getAddressType()) {
case ADDRESS_TYPE_INET:
reinterpret_cast<sockaddr_in *>(_platform_data)->sin_port = htons((short)port);
break;
case ADDRESS_TYPE_INET6:
reinterpret_cast<sockaddr_in6 *>(_platform_data)->sin6_port = htons((short)port);
break;
default:
return RESULT_OPERATION_FAIL;
}
return RESULT_OK;
}
u_result SocketAddress::setAddressFromString(const char * address_string, SocketAddress::address_type_t type)
{
int ans = 0;
int prevPort = getPort();
switch (type) {
case ADDRESS_TYPE_INET:
reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
ans = _inet_pton(AF_INET,
address_string,
&reinterpret_cast<sockaddr_in *>(_platform_data)->sin_addr);
break;
case ADDRESS_TYPE_INET6:
reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET6;
ans = _inet_pton(AF_INET6,
address_string,
&reinterpret_cast<sockaddr_in6 *>(_platform_data)->sin6_addr);
break;
default:
return RESULT_INVALID_DATA;
}
setPort(prevPort);
return ans<=0?RESULT_INVALID_DATA:RESULT_OK;
}
u_result SocketAddress::getAddressAsString(char * buffer, size_t buffersize) const
{
int net_family = reinterpret_cast<const sockaddr_storage *>(_platform_data)->ss_family;
const char *ans = NULL;
switch (net_family) {
case AF_INET:
ans = _inet_ntop(net_family, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr,
buffer, buffersize);
break;
case AF_INET6:
ans = _inet_ntop(net_family, &reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr,
buffer, buffersize);
break;
}
return ans<=0?RESULT_OPERATION_FAIL:RESULT_OK;
}
size_t SocketAddress::LoopUpHostName(const char * hostname, const char * sevicename, std::vector<SocketAddress> &addresspool , bool performDNS, SocketAddress::address_type_t type)
{
struct addrinfo hints;
struct addrinfo *result;
int ans;
_checkWSAStartup();
memset(&hints, 0, sizeof(struct addrinfo));
hints.ai_family = _halAddrTypeToOSType(type);
hints.ai_flags = AI_PASSIVE;
if (!performDNS) {
hints.ai_family |= AI_NUMERICSERV | AI_NUMERICHOST;
}
ans = getaddrinfo(hostname, sevicename, &hints, &result);
addresspool.clear();
if (ans != 0) {
// hostname loopup failed
return 0;
}
for (struct addrinfo * cursor = result; cursor != NULL; cursor = cursor->ai_next) {
if (cursor->ai_family == ADDRESS_TYPE_INET || cursor->ai_family == ADDRESS_TYPE_INET6) {
sockaddr_storage * storagebuffer = new sockaddr_storage;
assert(sizeof(sockaddr_storage) >= cursor->ai_addrlen);
memcpy(storagebuffer, cursor->ai_addr, cursor->ai_addrlen);
addresspool.push_back(SocketAddress(storagebuffer));
}
}
freeaddrinfo(result);
return addresspool.size();
}
u_result SocketAddress::getRawAddress(_u8 * buffer, size_t bufferSize) const
{
switch (getAddressType()) {
case ADDRESS_TYPE_INET:
if (bufferSize < sizeof(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr)) return RESULT_INSUFFICIENT_MEMORY;
memcpy(buffer, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr, sizeof(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr));
break;
case ADDRESS_TYPE_INET6:
if (bufferSize < sizeof(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr)) return RESULT_INSUFFICIENT_MEMORY;
memcpy(buffer, reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr, sizeof(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr));
break;
default:
return RESULT_OPERATION_FAIL;
}
return RESULT_OK;
}
void SocketAddress::setLoopbackAddress(SocketAddress::address_type_t type)
{
int prevPort = getPort();
switch (type) {
case ADDRESS_TYPE_INET:
{
sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
addrv4->sin_family = AF_INET;
addrv4->sin_addr.s_addr = htonl(INADDR_LOOPBACK);
}
break;
case ADDRESS_TYPE_INET6:
{
sockaddr_in6 * addrv6 = reinterpret_cast<sockaddr_in6 *>(_platform_data);
addrv6->sin6_family = AF_INET6;
addrv6->sin6_addr = in6addr_loopback;
}
break;
default:
return;
}
setPort(prevPort);
}
void SocketAddress::setBroadcastAddressIPv4()
{
int prevPort = getPort();
sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
addrv4->sin_family = AF_INET;
addrv4->sin_addr.s_addr = htonl(INADDR_BROADCAST);
setPort(prevPort);
}
void SocketAddress::setAnyAddress(SocketAddress::address_type_t type)
{
int prevPort = getPort();
switch (type) {
case ADDRESS_TYPE_INET:
{
sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
addrv4->sin_family = AF_INET;
addrv4->sin_addr.s_addr = htonl(INADDR_ANY);
}
break;
case ADDRESS_TYPE_INET6:
{
sockaddr_in6 * addrv6 = reinterpret_cast<sockaddr_in6 *>(_platform_data);
addrv6->sin6_family = AF_INET6;
addrv6->sin6_addr = in6addr_any;
}
break;
default:
return;
}
setPort(prevPort);
}
}}
///--------------------------------
namespace rp { namespace arch { namespace net{
using namespace rp::net;
class _single_thread StreamSocketImpl : public StreamSocket
{
public:
StreamSocketImpl(SOCKET fd)
: _socket_fd(fd)
{
assert(fd>=0);
int bool_true = 1;
::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR , (char *)&bool_true, (int)sizeof(bool_true) );
enableNoDelay(true);
this->setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH);
}
virtual ~StreamSocketImpl()
{
closesocket(_socket_fd);
}
virtual void dispose()
{
delete this;
}
virtual u_result bind(const SocketAddress & localaddr)
{
const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(localaddr.getPlatformData());
assert(addr);
int ans = ::bind(_socket_fd, addr, (int)sizeof(sockaddr_storage));
if (ans) {
return RESULT_OPERATION_FAIL;
} else {
return RESULT_OK;
}
}
virtual u_result getLocalAddress(SocketAddress & localaddr)
{
struct sockaddr * addr = reinterpret_cast<struct sockaddr *>( const_cast<void *>(localaddr.getPlatformData())); //donnot do this at home...
assert(addr);
int actualsize = sizeof(sockaddr_storage);
int ans = ::getsockname(_socket_fd, addr, &actualsize);
assert(actualsize <= sizeof(sockaddr_storage));
assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
return ans?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk)
{
int ans;
timeval tv;
tv.tv_sec = timeout / 1000;
tv.tv_usec = (timeout % 1000) * 1000;
if (msk & SOCKET_DIR_RD) {
ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, (char *)&tv, (int)sizeof(tv) );
if (ans) return RESULT_OPERATION_FAIL;
}
if (msk & SOCKET_DIR_WR) {
ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, (char *)&tv, (int)sizeof(tv) );
if (ans) return RESULT_OPERATION_FAIL;
}
return RESULT_OK;
}
virtual u_result connect(const SocketAddress & pairAddress)
{
const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(pairAddress.getPlatformData());
int ans = ::connect(_socket_fd, addr, (int)sizeof(sockaddr_storage));
if (!ans) return RESULT_OK;
switch (WSAGetLastError()) {
case WSAEAFNOSUPPORT:
return RESULT_OPERATION_NOT_SUPPORT;
#if 0
case EINPROGRESS:
return RESULT_OK; //treat async connection as good status
#endif
case WSAETIMEDOUT:
return RESULT_OPERATION_TIMEOUT;
default:
return RESULT_OPERATION_FAIL;
}
}
virtual u_result listen(int backlog)
{
int ans = ::listen( _socket_fd, backlog);
return ans?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual StreamSocket * accept(SocketAddress * pairAddress)
{
int addrsize;
addrsize = sizeof(sockaddr_storage);
SOCKET pair_socket = ::accept( _socket_fd, pairAddress?reinterpret_cast<struct sockaddr *>(const_cast<void *>(pairAddress->getPlatformData())):NULL
, &addrsize);
if (pair_socket>=0) {
return new StreamSocketImpl(pair_socket);
} else {
return NULL;
}
}
virtual u_result waitforIncomingConnection(_u32 timeout)
{
return waitforData(timeout);
}
virtual u_result send(const void * buffer, size_t len)
{
int ans = ::send( _socket_fd, (const char *)buffer, len, 0);
if (ans != SOCKET_ERROR ) {
assert(ans == (int)len);
return RESULT_OK;
} else {
switch(WSAGetLastError()) {
case WSAETIMEDOUT:
return RESULT_OPERATION_TIMEOUT;
default:
return RESULT_OPERATION_FAIL;
}
}
}
virtual u_result recv(void *buf, size_t len, size_t & recv_len)
{
int ans = ::recv( _socket_fd, (char *)buf, len, 0);
//::setsockopt(_socket_fd, IPPROTO_TCP, TCP_QUICKACK, (const char *)1, sizeof(int));
//::setsockopt(_socket_fd, IPPROTO_TCP, TCP_QUICKACK, (int[]){1}, sizeof(int))
if (ans == SOCKET_ERROR) {
recv_len = 0;
switch(WSAGetLastError()) {
case WSAETIMEDOUT:
return RESULT_OPERATION_TIMEOUT;
default:
return RESULT_OPERATION_FAIL;
}
} else {
recv_len = ans;
return RESULT_OK;
}
}
virtual u_result getPeerAddress(SocketAddress & peerAddr)
{
struct sockaddr * addr = reinterpret_cast<struct sockaddr *>(const_cast<void *>(peerAddr.getPlatformData())); //donnot do this at home...
assert(addr);
int actualsize = (int)sizeof(sockaddr_storage);
int ans = ::getpeername(_socket_fd, addr, &actualsize);
assert(actualsize <= (int)sizeof(sockaddr_storage));
assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
return ans?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual u_result shutdown(socket_direction_mask mask)
{
int shutdw_opt ;
switch (mask) {
case SOCKET_DIR_RD:
shutdw_opt = SD_RECEIVE;
break;
case SOCKET_DIR_WR:
shutdw_opt = SD_SEND;
break;
case SOCKET_DIR_BOTH:
default:
shutdw_opt = SD_BOTH;
}
int ans = ::shutdown(_socket_fd, shutdw_opt);
return ans?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual u_result enableKeepAlive(bool enable)
{
int bool_true = enable?1:0;
return ::setsockopt( _socket_fd, SOL_SOCKET, SO_KEEPALIVE , (const char *)&bool_true, (int)sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual u_result enableNoDelay(bool enable )
{
int bool_true = enable?1:0;
return ::setsockopt( _socket_fd, IPPROTO_TCP, TCP_NODELAY, (const char *)&bool_true, (int)sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual u_result waitforSent(_u32 timeout )
{
fd_set wrset;
FD_ZERO(&wrset);
FD_SET(_socket_fd, &wrset);
timeval tv;
tv.tv_sec = timeout / 1000;
tv.tv_usec = (timeout % 1000) * 1000;
int ans = ::select(NULL, NULL, &wrset, NULL, &tv);
switch (ans) {
case 1:
// fired
return RESULT_OK;
case 0:
// timeout
return RESULT_OPERATION_TIMEOUT;
default:
delay(0); //relax cpu
return RESULT_OPERATION_FAIL;
}
}
virtual u_result waitforData(_u32 timeout )
{
fd_set rdset;
FD_ZERO(&rdset);
FD_SET(_socket_fd, &rdset);
timeval tv;
tv.tv_sec = timeout / 1000;
tv.tv_usec = (timeout % 1000) * 1000;
int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv);
switch (ans) {
case 1:
// fired
return RESULT_OK;
case 0:
// timeout
return RESULT_OPERATION_TIMEOUT;
default:
delay(0); //relax cpu
return RESULT_OPERATION_FAIL;
}
}
protected:
SOCKET _socket_fd;
};
class _single_thread DGramSocketImpl : public DGramSocket
{
public:
DGramSocketImpl(SOCKET fd)
: _socket_fd(fd)
{
assert(fd>=0);
int bool_true = 1;
::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR | SO_BROADCAST , (char *)&bool_true, (int)sizeof(bool_true) );
setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH);
}
virtual ~DGramSocketImpl()
{
closesocket(_socket_fd);
}
virtual void dispose()
{
delete this;
}
virtual u_result bind(const SocketAddress & localaddr)
{
const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(localaddr.getPlatformData());
assert(addr);
int ans = ::bind(_socket_fd, addr, (int)sizeof(sockaddr_storage));
if (ans) {
return RESULT_OPERATION_FAIL;
} else {
return RESULT_OK;
}
}
virtual u_result getLocalAddress(SocketAddress & localaddr)
{
struct sockaddr * addr = reinterpret_cast<struct sockaddr *>(const_cast<void *>((localaddr.getPlatformData()))); //donnot do this at home...
assert(addr);
int actualsize = (int)sizeof(sockaddr_storage);
int ans = ::getsockname(_socket_fd, addr, &actualsize);
assert(actualsize <= (int)sizeof(sockaddr_storage));
assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
return ans?RESULT_OPERATION_FAIL:RESULT_OK;
}
virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk)
{
int ans;
timeval tv;
tv.tv_sec = timeout / 1000;
tv.tv_usec = (timeout % 1000) * 1000;
if (msk & SOCKET_DIR_RD) {
ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, (const char *)&tv, (int)sizeof(tv) );
if (ans) return RESULT_OPERATION_FAIL;
}
if (msk & SOCKET_DIR_WR) {
ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, (const char *)&tv, (int)sizeof(tv) );
if (ans) return RESULT_OPERATION_FAIL;
}
return RESULT_OK;
}
virtual u_result waitforSent(_u32 timeout )
{
fd_set wrset;
FD_ZERO(&wrset);
FD_SET(_socket_fd, &wrset);
timeval tv;
tv.tv_sec = timeout / 1000;
tv.tv_usec = (timeout % 1000) * 1000;
int ans = ::select(NULL, NULL, &wrset, NULL, &tv);
switch (ans) {
case 1:
// fired
return RESULT_OK;
case 0:
// timeout
return RESULT_OPERATION_TIMEOUT;
default:
delay(0); //relax cpu
return RESULT_OPERATION_FAIL;
}
}
virtual u_result waitforData(_u32 timeout )
{
fd_set rdset;
FD_ZERO(&rdset);
FD_SET(_socket_fd, &rdset);
timeval tv;
tv.tv_sec = timeout / 1000;
tv.tv_usec = (timeout % 1000) * 1000;
int ans = ::select(NULL, &rdset, NULL, NULL, &tv);
switch (ans) {
case 1:
// fired
return RESULT_OK;
case 0:
// timeout
return RESULT_OPERATION_TIMEOUT;
default:
delay(0); //relax cpu
return RESULT_OPERATION_FAIL;
}
}
virtual u_result sendTo(const SocketAddress & target, const void * buffer, size_t len)
{
const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(target.getPlatformData());
assert(addr);
int ans = ::sendto( _socket_fd, (const char *)buffer, (int)len, 0, addr, (int)sizeof(sockaddr_storage));
if (ans != SOCKET_ERROR) {
assert(ans == (int)len);
return RESULT_OK;
} else {
switch(WSAGetLastError()) {
case WSAETIMEDOUT:
return RESULT_OPERATION_TIMEOUT;
case WSAEMSGSIZE:
return RESULT_INVALID_DATA;
default:
return RESULT_OPERATION_FAIL;
}
}
}
virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr)
{
struct sockaddr * addr = (sourceAddr?reinterpret_cast<struct sockaddr *>(const_cast<void *>(sourceAddr->getPlatformData())):NULL);
int source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0);
int ans = ::recvfrom( _socket_fd, (char *)buf, (int)len, 0, addr, addr?&source_addr_size:NULL);
if (ans == SOCKET_ERROR) {
recv_len = 0;
int errCode = WSAGetLastError();
switch(errCode) {
case WSAETIMEDOUT:
return RESULT_OPERATION_TIMEOUT;
default:
return RESULT_OPERATION_FAIL;
}
} else {
recv_len = ans;
return RESULT_OK;
}
}
protected:
SOCKET _socket_fd;
};
}}}
namespace rp { namespace net{
static inline int _socketHalFamilyToOSFamily(SocketBase::socket_family_t family)
{
switch (family) {
case SocketBase::SOCKET_FAMILY_INET:
return AF_INET;
case SocketBase::SOCKET_FAMILY_INET6:
return AF_INET6;
case SocketBase::SOCKET_FAMILY_RAW:
return AF_UNSPEC; //win32 doesn't support RAW Packet
default:
assert(!"should not reach here");
return AF_INET; // force treating as IPv4 in release mode
}
}
StreamSocket * StreamSocket::CreateSocket(SocketBase::socket_family_t family)
{
_checkWSAStartup();
if (family == SOCKET_FAMILY_RAW) return NULL;
int socket_family = _socketHalFamilyToOSFamily(family);
int socket_fd = ::socket(socket_family, SOCK_STREAM, 0);
if (socket_fd == -1) return NULL;
StreamSocket * newborn = static_cast<StreamSocket *>(new rp::arch::net::StreamSocketImpl(socket_fd));
return newborn;
}
DGramSocket * DGramSocket::CreateSocket(SocketBase::socket_family_t family)
{
_checkWSAStartup();
int socket_family = _socketHalFamilyToOSFamily(family);
int socket_fd = ::socket(socket_family, (family==SOCKET_FAMILY_RAW)?SOCK_RAW:SOCK_DGRAM, 0);
if (socket_fd == -1) return NULL;
DGramSocket * newborn = static_cast<DGramSocket *>(new rp::arch::net::DGramSocketImpl(socket_fd));
return newborn;
}
}}

62
librviz_tutorial/sdk/src/arch/win32/timer.cpp Executable file
View File

@ -0,0 +1,62 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#include "sdkcommon.h"
#include <mmsystem.h>
#pragma comment(lib, "Winmm.lib")
namespace rp{ namespace arch{
static LARGE_INTEGER _current_freq;
void HPtimer_reset()
{
BOOL ans=QueryPerformanceFrequency(&_current_freq);
_current_freq.QuadPart/=1000;
}
_u32 getHDTimer()
{
LARGE_INTEGER current;
QueryPerformanceCounter(&current);
return (_u32)(current.QuadPart/_current_freq.QuadPart);
}
BEGIN_STATIC_CODE(timer_cailb)
{
HPtimer_reset();
}END_STATIC_CODE(timer_cailb)
}}

47
librviz_tutorial/sdk/src/arch/win32/timer.h Executable file
View File

@ -0,0 +1,47 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
#include "hal/types.h"
#define delay(x) ::Sleep(x)
namespace rp{ namespace arch{
void HPtimer_reset();
_u32 getHDTimer();
}}
#define getms() rp::arch::getHDTimer()

144
librviz_tutorial/sdk/src/arch/win32/winthread.hpp Executable file
View File

@ -0,0 +1,144 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#include "sdkcommon.h"
#include <process.h>
namespace rp{ namespace hal{
Thread Thread::create(thread_proc_t proc, void * data)
{
Thread newborn(proc, data);
newborn._handle = (_word_size_t)(
_beginthreadex(NULL, 0, (unsigned int (_stdcall * )( void * ))proc,
data, 0, NULL));
return newborn;
}
u_result Thread::terminate()
{
if (!this->_handle) return RESULT_OK;
if (TerminateThread( reinterpret_cast<HANDLE>(this->_handle), -1))
{
CloseHandle(reinterpret_cast<HANDLE>(this->_handle));
this->_handle = NULL;
return RESULT_OK;
}else
{
return RESULT_OPERATION_FAIL;
}
}
u_result Thread::setPriority( priority_val_t p)
{
if (!this->_handle) return RESULT_OPERATION_FAIL;
int win_priority = THREAD_PRIORITY_NORMAL;
switch(p)
{
case PRIORITY_REALTIME:
win_priority = THREAD_PRIORITY_TIME_CRITICAL;
break;
case PRIORITY_HIGH:
win_priority = THREAD_PRIORITY_HIGHEST;
break;
case PRIORITY_NORMAL:
win_priority = THREAD_PRIORITY_NORMAL;
break;
case PRIORITY_LOW:
win_priority = THREAD_PRIORITY_LOWEST;
break;
case PRIORITY_IDLE:
win_priority = THREAD_PRIORITY_IDLE;
break;
}
if (SetThreadPriority(reinterpret_cast<HANDLE>(this->_handle), win_priority))
{
return RESULT_OK;
}
return RESULT_OPERATION_FAIL;
}
Thread::priority_val_t Thread::getPriority()
{
if (!this->_handle) return PRIORITY_NORMAL;
int win_priority = ::GetThreadPriority(reinterpret_cast<HANDLE>(this->_handle));
if (win_priority == THREAD_PRIORITY_ERROR_RETURN)
{
return PRIORITY_NORMAL;
}
if (win_priority >= THREAD_PRIORITY_TIME_CRITICAL )
{
return PRIORITY_REALTIME;
}
else if (win_priority<THREAD_PRIORITY_TIME_CRITICAL && win_priority>=THREAD_PRIORITY_ABOVE_NORMAL)
{
return PRIORITY_HIGH;
}
else if (win_priority<THREAD_PRIORITY_ABOVE_NORMAL && win_priority>THREAD_PRIORITY_BELOW_NORMAL)
{
return PRIORITY_NORMAL;
}else if (win_priority<=THREAD_PRIORITY_BELOW_NORMAL && win_priority>THREAD_PRIORITY_IDLE)
{
return PRIORITY_LOW;
}else if (win_priority<=THREAD_PRIORITY_IDLE)
{
return PRIORITY_IDLE;
}
return PRIORITY_NORMAL;
}
u_result Thread::join(unsigned long timeout)
{
if (!this->_handle) return RESULT_OK;
switch ( WaitForSingleObject(reinterpret_cast<HANDLE>(this->_handle), timeout))
{
case WAIT_OBJECT_0:
CloseHandle(reinterpret_cast<HANDLE>(this->_handle));
this->_handle = NULL;
return RESULT_OK;
case WAIT_ABANDONED:
return RESULT_OPERATION_FAIL;
case WAIT_TIMEOUT:
return RESULT_OPERATION_TIMEOUT;
}
return RESULT_OK;
}
}}

88
librviz_tutorial/sdk/src/hal/abs_rxtx.h Executable file
View File

@ -0,0 +1,88 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
#include "hal/types.h"
namespace rp{ namespace hal{
class serial_rxtx
{
public:
enum{
ANS_OK = 0,
ANS_TIMEOUT = -1,
ANS_DEV_ERR = -2,
};
static serial_rxtx * CreateRxTx();
static void ReleaseRxTx( serial_rxtx * );
serial_rxtx():_is_serial_opened(false){}
virtual ~serial_rxtx(){}
virtual void flush( _u32 flags) = 0;
virtual bool bind(const char * portname, _u32 baudrate, _u32 flags = 0) = 0;
virtual bool open() = 0;
virtual void close() = 0;
virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL) = 0;
virtual int senddata(const unsigned char * data, size_t size) = 0;
virtual int recvdata(unsigned char * data, size_t size) = 0;
virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL) = 0;
virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL) = 0;
virtual size_t rxqueue_count() = 0;
virtual void setDTR() = 0;
virtual void clearDTR() = 0;
virtual void cancelOperation() {}
virtual bool isOpened()
{
return _is_serial_opened;
}
protected:
volatile bool _is_serial_opened;
};
}}

18
librviz_tutorial/sdk/src/hal/assert.h Executable file
View File

@ -0,0 +1,18 @@
#ifndef _INFRA_HAL_ASSERT_H
#define _INFRA_HAL_ASSERT_H
#ifdef WIN32
#include <crtdbg.h>
#ifndef assert
#define assert(x) _ASSERT(x)
#endif
#elif defined(_AVR_)
#define assert(x)
#elif defined(__GNUC__)
#ifndef assert
#define assert(x)
#endif
#else
#error assert.h cannot identify your platform
#endif
#endif

94
librviz_tutorial/sdk/src/hal/byteops.h Executable file
View File

@ -0,0 +1,94 @@
/*
* RoboPeak Project
* Copyright 2009 - 2013
*
* RPOS - Byte Operations
*
*/
#pragma once
// byte swapping operations for compiling time
#define __static_byteswap_16(x) ((_u16)( \
(((_u16)(x) & (_u16)0x00FFU) << 8) | \
(((_u16)(x) & (_u16)0xFF00U) >> 8)))
#define __static_byteswap_32(x) ((_u32)( \
(((_u32)(x) & (_u32)0x000000FFUL) << 24) | \
(((_u32)(x) & (_u32)0x0000FF00UL) << 8) | \
(((_u32)(x) & (_u32)0x00FF0000UL) >> 8) | \
(((_u32)(x) & (_u32)0xFF000000UL) >> 24)))
#define __static_byteswap_64(x) ((_u64)( \
(((_u64)(x) & (_u64)0x00000000000000ffULL) << 56) | \
(((_u64)(x) & (_u64)0x000000000000ff00ULL) << 40) | \
(((_u64)(x) & (_u64)0x0000000000ff0000ULL) << 24) | \
(((_u64)(x) & (_u64)0x00000000ff000000ULL) << 8) | \
(((_u64)(x) & (_u64)0x000000ff00000000ULL) >> 8) | \
(((_u64)(x) & (_u64)0x0000ff0000000000ULL) >> 24) | \
(((_u64)(x) & (_u64)0x00ff000000000000ULL) >> 40) | \
(((_u64)(x) & (_u64)0xff00000000000000ULL) >> 56)))
#define __fast_swap(a, b) do { (a) ^= (b); (b) ^= (a); (a) ^= (b); } while(0)
static inline _u16 __byteswap_16(_u16 x)
{
#ifdef __arch_byteswap_16
return __arch_byteswap_16(x);
#else
return __static_byteswap_16(x);
#endif
}
static inline _u32 __byteswap_32(_u32 x)
{
#ifdef __arch_byteswap_32
return __arch_byteswap_32(x);
#else
return __static_byteswap_32(x);
#endif
}
static inline _u64 __byteswap_64(_u64 x)
{
#ifdef __arch_byteswap_64
return __arch_byteswap_64(x);
#else
return __static_byteswap_64(x);
#endif
}
#ifdef float
static inline float __byteswap_float(float x)
{
#ifdef __arch_byteswap_float
return __arch_byteswap_float(x);
#else
_u8 * raw = (_u8 *)&x;
__fast_swap(raw[0], raw[3]);
__fast_swap(raw[1], raw[2]);
return x;
#endif
}
#endif
#ifdef double
static inline double __byteswap_double(double x)
{
#ifdef __arch_byteswap_double
return __arch_byteswap_double(x);
#else
_u8 * raw = (_u8 *)&x;
__fast_swap(raw[0], raw[7]);
__fast_swap(raw[1], raw[6]);
__fast_swap(raw[2], raw[5]);
__fast_swap(raw[3], raw[4]);
return x;
#endif
}
#endif

186
librviz_tutorial/sdk/src/hal/event.h Executable file
View File

@ -0,0 +1,186 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
namespace rp{ namespace hal{
class Event
{
public:
enum
{
EVENT_OK = 1,
EVENT_TIMEOUT = -1,
EVENT_FAILED = 0,
};
Event(bool isAutoReset = true, bool isSignal = false)
#ifdef _WIN32
: _event(NULL)
#else
: _is_signalled(isSignal)
, _isAutoReset(isAutoReset)
#endif
{
#ifdef _WIN32
_event = CreateEvent(NULL, isAutoReset?FALSE:TRUE, isSignal?TRUE:FALSE, NULL);
#else
pthread_mutex_init(&_cond_locker, NULL);
pthread_cond_init(&_cond_var, NULL);
#endif
}
~ Event()
{
release();
}
void set( bool isSignal = true )
{
if (isSignal){
#ifdef _WIN32
SetEvent(_event);
#else
pthread_mutex_lock(&_cond_locker);
if ( _is_signalled == false )
{
_is_signalled = true;
pthread_cond_signal(&_cond_var);
}
pthread_mutex_unlock(&_cond_locker);
#endif
}
else
{
#ifdef _WIN32
ResetEvent(_event);
#else
pthread_mutex_lock(&_cond_locker);
_is_signalled = false;
pthread_mutex_unlock(&_cond_locker);
#endif
}
}
unsigned long wait( unsigned long timeout = 0xFFFFFFFF )
{
#ifdef _WIN32
switch (WaitForSingleObject(_event, timeout==0xFFFFFFF?INFINITE:(DWORD)timeout))
{
case WAIT_FAILED:
return EVENT_FAILED;
case WAIT_OBJECT_0:
return EVENT_OK;
case WAIT_TIMEOUT:
return EVENT_TIMEOUT;
}
return EVENT_OK;
#else
unsigned long ans = EVENT_OK;
pthread_mutex_lock( &_cond_locker );
if ( !_is_signalled )
{
if (timeout == 0xFFFFFFFF){
pthread_cond_wait(&_cond_var,&_cond_locker);
}else
{
timespec wait_time;
timeval now;
gettimeofday(&now,NULL);
wait_time.tv_sec = timeout/1000 + now.tv_sec;
wait_time.tv_nsec = (timeout%1000)*1000000ULL + now.tv_usec*1000;
if (wait_time.tv_nsec >= 1000000000)
{
++wait_time.tv_sec;
wait_time.tv_nsec -= 1000000000;
}
switch (pthread_cond_timedwait(&_cond_var,&_cond_locker,&wait_time))
{
case 0:
// signalled
break;
case ETIMEDOUT:
// time up
ans = EVENT_TIMEOUT;
goto _final;
break;
default:
ans = EVENT_FAILED;
goto _final;
}
}
}
assert(_is_signalled);
if ( _isAutoReset )
{
_is_signalled = false;
}
_final:
pthread_mutex_unlock( &_cond_locker );
return ans;
#endif
}
protected:
void release()
{
#ifdef _WIN32
CloseHandle(_event);
#else
pthread_mutex_destroy(&_cond_locker);
pthread_cond_destroy(&_cond_var);
#endif
}
#ifdef _WIN32
HANDLE _event;
#else
pthread_cond_t _cond_var;
pthread_mutex_t _cond_locker;
bool _is_signalled;
bool _isAutoReset;
#endif
};
}}

186
librviz_tutorial/sdk/src/hal/locker.h Executable file
View File

@ -0,0 +1,186 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
namespace rp{ namespace hal{
class Locker
{
public:
enum LOCK_STATUS
{
LOCK_OK = 1,
LOCK_TIMEOUT = -1,
LOCK_FAILED = 0
};
Locker(){
#ifdef _WIN32
_lock = NULL;
#endif
init();
}
~Locker()
{
release();
}
Locker::LOCK_STATUS lock(unsigned long timeout = 0xFFFFFFFF)
{
#ifdef _WIN32
switch (WaitForSingleObject(_lock, timeout==0xFFFFFFF?INFINITE:(DWORD)timeout))
{
case WAIT_ABANDONED:
return LOCK_FAILED;
case WAIT_OBJECT_0:
return LOCK_OK;
case WAIT_TIMEOUT:
return LOCK_TIMEOUT;
}
#else
#ifdef _MACOS
if (timeout !=0 ) {
if (pthread_mutex_lock(&_lock) == 0) return LOCK_OK;
}
#else
if (timeout == 0xFFFFFFFF){
if (pthread_mutex_lock(&_lock) == 0) return LOCK_OK;
}
#endif
else if (timeout == 0)
{
if (pthread_mutex_trylock(&_lock) == 0) return LOCK_OK;
}
#ifndef _MACOS
else
{
timespec wait_time;
timeval now;
gettimeofday(&now,NULL);
wait_time.tv_sec = timeout/1000 + now.tv_sec;
wait_time.tv_nsec = (timeout%1000)*1000000 + now.tv_usec*1000;
if (wait_time.tv_nsec >= 1000000000)
{
++wait_time.tv_sec;
wait_time.tv_nsec -= 1000000000;
}
switch (pthread_mutex_timedlock(&_lock,&wait_time))
{
case 0:
return LOCK_OK;
case ETIMEDOUT:
return LOCK_TIMEOUT;
}
}
#endif
#endif
return LOCK_FAILED;
}
void unlock()
{
#ifdef _WIN32
ReleaseMutex(_lock);
#else
pthread_mutex_unlock(&_lock);
#endif
}
#ifdef _WIN32
HANDLE getLockHandle()
{
return _lock;
}
#else
pthread_mutex_t *getLockHandle()
{
return &_lock;
}
#endif
protected:
void init()
{
#ifdef _WIN32
_lock = CreateMutex(NULL,FALSE,NULL);
#else
pthread_mutex_init(&_lock, NULL);
#endif
}
void release()
{
unlock(); //force unlock before release
#ifdef _WIN32
if (_lock) CloseHandle(_lock);
_lock = NULL;
#else
pthread_mutex_destroy(&_lock);
#endif
}
#ifdef _WIN32
HANDLE _lock;
#else
pthread_mutex_t _lock;
#endif
};
class AutoLocker
{
public :
AutoLocker(Locker &l): _binded(l)
{
_binded.lock();
}
void forceUnlock() {
_binded.unlock();
}
~AutoLocker() {_binded.unlock();}
Locker & _binded;
};
}}

151
librviz_tutorial/sdk/src/hal/socket.h Executable file
View File

@ -0,0 +1,151 @@
/*
* RoboPeak Project
* HAL Layer - Socket Interface
* Copyright 2009 - 2013 RoboPeak Project
*/
#pragma once
#include <vector>
namespace rp{ namespace net {
class _single_thread SocketAddress
{
public:
enum address_type_t {
ADDRESS_TYPE_UNSPEC = 0,
ADDRESS_TYPE_INET = 1,
ADDRESS_TYPE_INET6 = 2,
};
public:
SocketAddress();
SocketAddress(const char * addrString, int port, address_type_t = ADDRESS_TYPE_INET);
// do not use this function, internal usage
SocketAddress(void * platform_data);
SocketAddress(const SocketAddress &);
SocketAddress & operator = (const SocketAddress &);
virtual ~SocketAddress();
virtual int getPort() const;
virtual u_result setPort(int port);
virtual u_result setAddressFromString(const char * address_string, address_type_t = ADDRESS_TYPE_INET);
virtual u_result getAddressAsString(char * buffer, size_t buffersize) const;
virtual address_type_t getAddressType() const;
virtual u_result getRawAddress(_u8 * buffer, size_t bufferSize) const;
const void * getPlatformData() const {
return _platform_data;
}
virtual void setLoopbackAddress(address_type_t = ADDRESS_TYPE_INET);
virtual void setBroadcastAddressIPv4();
virtual void setAnyAddress(address_type_t = ADDRESS_TYPE_INET);
public:
static size_t LoopUpHostName(const char * hostname, const char * sevicename, std::vector<SocketAddress> &addresspool , bool performDNS = true, address_type_t = ADDRESS_TYPE_INET);
protected:
void * _platform_data;
};
class SocketBase
{
public:
enum socket_family_t {
SOCKET_FAMILY_INET = 0,
SOCKET_FAMILY_INET6 = 1,
SOCKET_FAMILY_RAW = 2,
};
enum socket_direction_mask {
SOCKET_DIR_RD = 0x1,
SOCKET_DIR_WR = 0x2,
SOCKET_DIR_BOTH = (SOCKET_DIR_RD | SOCKET_DIR_WR),
};
enum {
DEFAULT_SOCKET_TIMEOUT = 10000, //10sec
};
virtual ~SocketBase() {}
virtual void dispose() = 0;
virtual u_result bind(const SocketAddress & ) = 0;
virtual u_result getLocalAddress(SocketAddress & ) = 0;
virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk = SOCKET_DIR_BOTH) = 0;
virtual u_result waitforSent(_u32 timeout = DEFAULT_SOCKET_TIMEOUT) = 0;
virtual u_result waitforData(_u32 timeout = DEFAULT_SOCKET_TIMEOUT) = 0;
protected:
SocketBase() {}
};
class _single_thread StreamSocket : public SocketBase
{
public:
enum {
MAX_BACKLOG = 128,
};
static StreamSocket * CreateSocket(socket_family_t family = SOCKET_FAMILY_INET);
virtual u_result connect(const SocketAddress & pairAddress) = 0;
virtual u_result listen(int backlog = MAX_BACKLOG) = 0;
virtual StreamSocket * accept(SocketAddress * pairAddress = NULL) = 0;
virtual u_result waitforIncomingConnection(_u32 timeout = DEFAULT_SOCKET_TIMEOUT) = 0;
virtual u_result send(const void * buffer, size_t len) = 0;
virtual u_result recv(void *buf, size_t len, size_t & recv_len) = 0;
virtual u_result getPeerAddress(SocketAddress & ) = 0;
virtual u_result shutdown(socket_direction_mask mask) = 0;
virtual u_result enableKeepAlive(bool enable = true) = 0;
virtual u_result enableNoDelay(bool enable = true) = 0;
protected:
virtual ~StreamSocket() {} // use dispose();
StreamSocket() {}
};
class _single_thread DGramSocket: public SocketBase
{
public:
static DGramSocket * CreateSocket(socket_family_t family = SOCKET_FAMILY_INET);
virtual u_result sendTo(const SocketAddress & target, const void * buffer, size_t len) = 0;
virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr = NULL) = 0;
protected:
virtual ~DGramSocket() {} // use dispose();
DGramSocket() {}
};
}}

48
librviz_tutorial/sdk/src/hal/thread.cpp Executable file
View File

@ -0,0 +1,48 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#include "sdkcommon.h"
#include "hal/thread.h"
#if defined(_WIN32)
#include "arch/win32/winthread.hpp"
#elif defined(_MACOS)
#include "arch/macOS/thread.hpp"
#elif defined(__GNUC__)
#include "arch/linux/thread.hpp"
#else
#error no threading implemention found for this platform.
#endif

87
librviz_tutorial/sdk/src/hal/thread.h Executable file
View File

@ -0,0 +1,87 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
#include "hal/types.h"
#define CLASS_THREAD(c , x ) \
rp::hal::Thread::create_member<c, &c::x>(this )
namespace rp{ namespace hal{
class Thread
{
public:
enum priority_val_t
{
PRIORITY_REALTIME = 0,
PRIORITY_HIGH = 1,
PRIORITY_NORMAL = 2,
PRIORITY_LOW = 3,
PRIORITY_IDLE = 4,
};
template <class T, u_result (T::*PROC)(void)>
static Thread create_member(T * pthis)
{
return create(_thread_thunk<T,PROC>, pthis);
}
template <class T, u_result (T::*PROC)(void) >
static _word_size_t THREAD_PROC _thread_thunk(void * data)
{
return (static_cast<T *>(data)->*PROC)();
}
static Thread create(thread_proc_t proc, void * data = NULL );
public:
~Thread() { }
Thread(): _data(NULL),_func(NULL),_handle(0) {}
_word_size_t getHandle(){ return _handle;}
u_result terminate();
void *getData() { return _data;}
u_result join(unsigned long timeout = -1);
u_result setPriority( priority_val_t p);
priority_val_t getPriority();
bool operator== ( const Thread & right) { return this->_handle == right._handle; }
protected:
Thread( thread_proc_t proc, void * data ): _data(data),_func(proc), _handle(0) {}
void * _data;
thread_proc_t _func;
_word_size_t _handle;
};
}}

119
librviz_tutorial/sdk/src/hal/types.h Executable file
View File

@ -0,0 +1,119 @@
/*
* Common Data Types for RP
*/
#ifndef _INFRA_HAL_TYPES_H_
#define _INFRA_HAL_TYPES_H_
//Basic types
//
#ifdef WIN32
//fake stdint.h for VC only
typedef signed char int8_t;
typedef unsigned char uint8_t;
typedef __int16 int16_t;
typedef unsigned __int16 uint16_t;
typedef __int32 int32_t;
typedef unsigned __int32 uint32_t;
typedef __int64 int64_t;
typedef unsigned __int64 uint64_t;
#define RPMODULE_EXPORT __declspec(dllexport)
#define RPMODULE_IMPORT __declspec(dllimport)
#else
#include <stdint.h>
#define RPMODULE_EXPORT
#define RPMODULE_IMPORT
#endif
//based on stdint.h
typedef int8_t _s8;
typedef uint8_t _u8;
typedef int16_t _s16;
typedef uint16_t _u16;
typedef int32_t _s32;
typedef uint32_t _u32;
typedef int64_t _s64;
typedef uint64_t _u64;
#define __small_endian
#ifndef __GNUC__
#define __attribute__(x)
#endif
// The _word_size_t uses actual data bus width of the current CPU
#ifdef _AVR_
typedef _u8 _word_size_t;
#define THREAD_PROC
#elif defined (WIN64)
typedef _u64 _word_size_t;
#define THREAD_PROC __stdcall
#elif defined (WIN32)
typedef _u32 _word_size_t;
#define THREAD_PROC __stdcall
#elif defined (__GNUC__)
typedef unsigned long _word_size_t;
#define THREAD_PROC
#elif defined (__ICCARM__)
typedef _u32 _word_size_t;
#define THREAD_PROC
#endif
#define __le
#define __be
#define _multi_thread
#define _single_thread
typedef uint32_t u_result;
#define RESULT_OK 0
#define RESULT_FAIL_BIT 0x80000000
#define RESULT_ALREADY_DONE 0x20
#define RESULT_INVALID_DATA (0x8000 | RESULT_FAIL_BIT)
#define RESULT_OPERATION_FAIL (0x8001 | RESULT_FAIL_BIT)
#define RESULT_OPERATION_TIMEOUT (0x8002 | RESULT_FAIL_BIT)
#define RESULT_OPERATION_STOP (0x8003 | RESULT_FAIL_BIT)
#define RESULT_OPERATION_NOT_SUPPORT (0x8004 | RESULT_FAIL_BIT)
#define RESULT_FORMAT_NOT_SUPPORT (0x8005 | RESULT_FAIL_BIT)
#define RESULT_INSUFFICIENT_MEMORY (0x8006 | RESULT_FAIL_BIT)
#define RESULT_OPERATION_ABORTED (0x8007 | RESULT_FAIL_BIT)
#define RESULT_NOT_FOUND (0x8008 | RESULT_FAIL_BIT)
#define RESULT_RECONNECTING (0x8009 | RESULT_FAIL_BIT)
#define IS_OK(x) ( ((x) & RESULT_FAIL_BIT) == 0 )
#define IS_FAIL(x) ( ((x) & RESULT_FAIL_BIT) )
typedef _word_size_t (THREAD_PROC * thread_proc_t ) ( void * );
#if defined (_BUILD_AS_DLL)
#if defined (_BUILD_DLL_EXPORT)
#define RPMODULE_IMPEXP RPMODULE_EXPORT
#else
#define RPMODULE_IMPEXP RPMODULE_IMPORT
#endif
#else
#define RPMODULE_IMPEXP
#endif
#endif

67
librviz_tutorial/sdk/src/hal/util.h Executable file
View File

@ -0,0 +1,67 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
//------
/* _countof helper */
#if !defined(_countof)
#if !defined(__cplusplus)
#define _countof(_Array) (sizeof(_Array) / sizeof(_Array[0]))
#else
extern "C++"
{
template <typename _CountofType, size_t _SizeOfArray>
char (*__countof_helper( _CountofType (&_Array)[_SizeOfArray]))[_SizeOfArray];
#define _countof(_Array) sizeof(*__countof_helper(_Array))
}
#endif
#endif
/* _offsetof helper */
#if !defined(offsetof)
#define offsetof(_structure, _field) ((_word_size_t)&(((_structure *)0x0)->_field))
#endif
#define BEGIN_STATIC_CODE( _blockname_ ) \
static class _static_code_##_blockname_ { \
public: \
_static_code_##_blockname_ ()
#define END_STATIC_CODE( _blockname_ ) \
} _instance_##_blockname_;

2181
librviz_tutorial/sdk/src/rplidar_driver.cpp Executable file
View File

File diff suppressed because it is too large Load Diff

85
librviz_tutorial/sdk/src/rplidar_driver_TCP.h Executable file
View File

@ -0,0 +1,85 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
namespace rp { namespace standalone{ namespace rplidar {
class TCPChannelDevice :public ChannelDevice
{
public:
rp::net::StreamSocket * _binded_socket;
TCPChannelDevice():_binded_socket(rp::net::StreamSocket::CreateSocket()){}
bool bind(const char * ipStr, uint32_t port)
{
rp::net::SocketAddress socket(ipStr, port);
return IS_OK(_binded_socket->connect(socket));
}
void close()
{
_binded_socket->dispose();
_binded_socket = NULL;
}
bool waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL)
{
if(returned_size)
*returned_size = data_count;
return (_binded_socket->waitforData(timeout) == RESULT_OK);
}
int senddata(const _u8 * data, size_t size)
{
return _binded_socket->send(data, size) ;
}
int recvdata(unsigned char * data, size_t size)
{
size_t lenRec = 0;
_binded_socket->recv(data, size, lenRec);
return lenRec;
}
};
class RPlidarDriverTCP : public RPlidarDriverImplCommon
{
public:
RPlidarDriverTCP();
virtual ~RPlidarDriverTCP();
virtual u_result connect(const char * ipStr, _u32 port, _u32 flag = 0);
virtual void disconnect();
};
}}}

128
librviz_tutorial/sdk/src/rplidar_driver_impl.h Executable file
View File

@ -0,0 +1,128 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
namespace rp { namespace standalone{ namespace rplidar {
class RPlidarDriverImplCommon : public RPlidarDriver
{
public:
virtual bool isConnected();
virtual u_result reset(_u32 timeout = DEFAULT_TIMEOUT);
virtual u_result getAllSupportedScanModes(std::vector<RplidarScanMode>& outModes, _u32 timeoutInMs = DEFAULT_TIMEOUT);
virtual u_result getTypicalScanMode(_u16& outMode, _u32 timeoutInMs = DEFAULT_TIMEOUT);
virtual u_result checkSupportConfigCommands(bool& outSupport, _u32 timeoutInMs = DEFAULT_TIMEOUT);
virtual u_result getScanModeCount(_u16& modeCount, _u32 timeoutInMs = DEFAULT_TIMEOUT);
virtual u_result getLidarSampleDuration(float& sampleDurationRes, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT);
virtual u_result getMaxDistance(float &maxDistance, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT);
virtual u_result getScanModeAnsType(_u8 &ansType, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT);
virtual u_result getScanModeName(char* modeName, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT);
virtual u_result getLidarConf(_u32 type, std::vector<_u8> &outputBuf, const std::vector<_u8> &reserve = std::vector<_u8>(), _u32 timeout = DEFAULT_TIMEOUT);
virtual u_result startScan(bool force, bool useTypicalScan, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL);
virtual u_result startScanExpress(bool force, _u16 scanMode, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL, _u32 timeout = DEFAULT_TIMEOUT);
virtual u_result getHealth(rplidar_response_device_health_t & health, _u32 timeout = DEFAULT_TIMEOUT);
virtual u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT);
virtual u_result getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout = DEFAULT_TIMEOUT);
virtual u_result setMotorPWM(_u16 pwm);
virtual u_result startMotor();
virtual u_result stopMotor();
virtual u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT);
virtual u_result getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode);
virtual u_result getFrequency(const RplidarScanMode& scanMode, size_t count, float & frequency);
virtual u_result startScanNormal(bool force, _u32 timeout = DEFAULT_TIMEOUT);
virtual u_result checkExpressScanSupported(bool & support, _u32 timeout = DEFAULT_TIMEOUT);
virtual u_result stop(_u32 timeout = DEFAULT_TIMEOUT);
virtual u_result grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT);
virtual u_result grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT);
virtual u_result ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count);
virtual u_result ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count);
virtual u_result getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count);
virtual u_result getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count);
protected:
virtual u_result _sendCommand(_u8 cmd, const void * payload = NULL, size_t payloadsize = 0);
void _disableDataGrabbing();
virtual u_result _waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout = DEFAULT_TIMEOUT);
virtual u_result _cacheScanData();
virtual u_result _waitScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT);
virtual u_result _waitNode(rplidar_response_measurement_node_t * node, _u32 timeout = DEFAULT_TIMEOUT);
virtual u_result _cacheCapsuledScanData();
virtual u_result _waitCapsuledNode(rplidar_response_capsule_measurement_nodes_t & node, _u32 timeout = DEFAULT_TIMEOUT);
virtual void _capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount);
//FW1.23
virtual u_result _cacheUltraCapsuledScanData();
virtual u_result _waitUltraCapsuledNode(rplidar_response_ultra_capsule_measurement_nodes_t & node, _u32 timeout = DEFAULT_TIMEOUT);
virtual void _ultraCapsuleToNormal(const rplidar_response_ultra_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount);
virtual u_result _cacheHqScanData();
virtual u_result _waitHqNode(rplidar_response_hq_capsule_measurement_nodes_t & node, _u32 timeout = DEFAULT_TIMEOUT);
virtual void _HqToNormal(const rplidar_response_hq_capsule_measurement_nodes_t & node_hq, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount);
bool _isConnected;
bool _isScanning;
bool _isSupportingMotorCtrl;
rplidar_response_measurement_node_hq_t _cached_scan_node_hq_buf[8192];
size_t _cached_scan_node_hq_count;
rplidar_response_measurement_node_hq_t _cached_scan_node_hq_buf_for_interval_retrieve[8192];
size_t _cached_scan_node_hq_count_for_interval_retrieve;
_u16 _cached_sampleduration_std;
_u16 _cached_sampleduration_express;
rplidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata;
rplidar_response_ultra_capsule_measurement_nodes_t _cached_previous_ultracapsuledata;
rplidar_response_hq_capsule_measurement_nodes_t _cached_previous_Hqdata;
bool _is_previous_capsuledataRdy;
bool _is_previous_HqdataRdy;
rp::hal::Locker _lock;
rp::hal::Event _dataEvt;
rp::hal::Thread _cachethread;
protected:
RPlidarDriverImplCommon();
virtual ~RPlidarDriverImplCommon() {}
};
}}}

106
librviz_tutorial/sdk/src/rplidar_driver_serial.h Executable file
View File

@ -0,0 +1,106 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#pragma once
namespace rp { namespace standalone{ namespace rplidar {
class SerialChannelDevice :public ChannelDevice
{
public:
rp::hal::serial_rxtx * _rxtxSerial;
bool _closePending;
SerialChannelDevice():_rxtxSerial(rp::hal::serial_rxtx::CreateRxTx()){}
bool bind(const char * portname, uint32_t baudrate)
{
_closePending = false;
return _rxtxSerial->bind(portname, baudrate);
}
bool open()
{
return _rxtxSerial->open();
}
void close()
{
_closePending = true;
_rxtxSerial->cancelOperation();
_rxtxSerial->close();
}
void flush()
{
_rxtxSerial->flush(0);
}
bool waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL)
{
if (_closePending) return false;
return (_rxtxSerial->waitfordata(data_count, timeout, returned_size) == rp::hal::serial_rxtx::ANS_OK);
}
int senddata(const _u8 * data, size_t size)
{
return _rxtxSerial->senddata(data, size) ;
}
int recvdata(unsigned char * data, size_t size)
{
size_t lenRec = 0;
lenRec = _rxtxSerial->recvdata(data, size);
return lenRec;
}
void setDTR()
{
_rxtxSerial->setDTR();
}
void clearDTR()
{
_rxtxSerial->clearDTR();
}
void ReleaseRxTx()
{
rp::hal::serial_rxtx::ReleaseRxTx(_rxtxSerial);
}
};
class RPlidarDriverSerial : public RPlidarDriverImplCommon
{
public:
RPlidarDriverSerial();
virtual ~RPlidarDriverSerial();
virtual u_result connect(const char * port_path, _u32 baudrate, _u32 flag = 0);
virtual void disconnect();
};
}}}

50
librviz_tutorial/sdk/src/sdkcommon.h Executable file
View File

@ -0,0 +1,50 @@
/*
* RPLIDAR SDK
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#if defined(_WIN32)
#include "arch\win32\arch_win32.h"
#elif defined(_MACOS)
#include "arch/macOS/arch_macOS.h"
#elif defined(__GNUC__)
#include "arch/linux/arch_linux.h"
#else
#error "unsupported target"
#endif
#include "hal/types.h"
#include "hal/assert.h"
#include "rplidar.h"
#include "hal/util.h"

25
librviz_tutorial/src/doc/conf.py Executable file
View File

@ -0,0 +1,25 @@
import sys, os
sys.path += [ os.path.abspath( '.' )]
extensions = [ 'sphinx.ext.extlinks',
'tutorialformatter' ]
# The master toctree document.
master_doc = 'index'
# The suffix of source filenames.
source_suffix = '.rst'
project = u'librviz_tutorial'
copyright = u'2012, Willow Garage, Inc'
# If true, sectionauthor and moduleauthor directives will be shown in the
# output. They are ignored by default.
show_authors = True
# The name of the Pygments (syntax highlighting) style to use.
pygments_style = 'sphinx'
extlinks = {'codedir': ('https://github.com/ros-visualization/visualization_tutorials/blob/hydro-devel/librviz_tutorial/%s', '')}

67
librviz_tutorial/src/doc/index.rst Executable file
View File

@ -0,0 +1,67 @@
Librviz Tutorial
================
Overview
--------
RViz is not just a visualizer application, it is also a library! Much
of RViz's functionality can be accessed within your own application by
linking against librviz.so (or whatever your OS likes to call it).
This tutorial shows a very simple example of creating a 3D visualizer
widget (rviz::RenderPanel), programmatically creating a new Grid
display within it, then using Qt slider controls to adjust a couple of
the grid's properties. The app is called "myviz".
The source code for this tutorial is in the librviz_tutorial
package. You can check out the source directly or (if you use Ubuntu)
you can just apt-get install the pre-compiled Debian package like so::
sudo apt-get install ros-hydro-visualization-tutorials
The running application looks like this:
.. image:: myviz.png
The Code
--------
The code for myviz is in these files:
:codedir:`src/main.cpp`,
:codedir:`src/myviz.h`, and
:codedir:`src/myviz.cpp`.
main.cpp
^^^^^^^^
The full text of main.cpp is here: :codedir:`src/main.cpp`
.. tutorial-formatter:: ../main.cpp
myviz.h
^^^^^^^
The full text of myviz.h is here: :codedir:`src/myviz.h`
.. tutorial-formatter:: ../myviz.h
myviz.cpp
^^^^^^^^^
The full text of myviz.cpp is here: :codedir:`src/myviz.cpp`
.. tutorial-formatter:: ../myviz.cpp
Building
--------
The full text of CMakeLists.txt is here: :codedir:`CMakeLists.txt`
.. tutorial-formatter:: ../../CMakeLists.txt
Running
-------
Just type::
rosrun librviz_tutorial myviz

BIN
librviz_tutorial/src/doc/myviz.png Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 13 KiB

132
librviz_tutorial/src/doc/tutorialformatter.py Executable file
View File

@ -0,0 +1,132 @@
"""
tutorialformatter
===========================
This extension provides a directive to include a source code file
in a document, but with certain comments from the file formatted
as regular document text. This allows code for a tutorial to look like:
/// BEGIN_TUTORIAL
/// This next line adds one.
i = i + 1;
/// Then we need to double it.
i = i * 2;
/// END_TUTORIAL
And have it formatted as
This next line adds one.::
i = i + 1;
Then we need to double it.::
i = i * 2;
The special-looking comment character sequence at the start of
each text line can be anything not starting or ending with
whitespace. tutorialformatter starts by scanning the file for the
string BEGIN_TUTORIAL. When it finds it, it takes all the
characters before BEGIN_TUTORIAL on that line, strips whitespace
from the left, and uses that as the text marker. So this would
also be fine:
#My Tutorial# BEGIN_TUTORIAL
#My Tutorial# This next line adds one.
i = i + 1
#My Tutorial# Then we need to double it.
i = i * 2
#My Tutorial# END_TUTORIAL
.. moduleauthor:: Dave Hershberger <hersh@willowgarage.com>
"""
__version__ = '0.1.0'
import os
from docutils.parsers import rst
from docutils.parsers.rst.directives import flag, unchanged
from docutils.statemachine import string2lines
from pygments.lexers import get_lexer_for_filename
class TutorialFormatterDirective(rst.Directive):
has_content = False
final_argument_whitespace = True
required_arguments = 1
option_spec = dict(shell=flag, prompt=flag, nostderr=flag,
in_srcdir=flag, extraargs=unchanged,
until=unchanged)
def run(self):
filename = self.arguments[0]
text_tag = None
tag_len = 0
filepath = self.state.document.settings.env.srcdir
absfilename = os.path.join( filepath, filename )
if absfilename.endswith('.h'):
language = 'c++'
elif absfilename.endswith('CMakeLists.txt'):
language = 'cmake'
else:
try:
language = get_lexer_for_filename( absfilename ).name.lower()
if language == 'text only':
language = 'none'
except:
language = 'none'
code_prefix = '\n.. code-block:: ' + language + '\n\n'
code_suffix = '\n'
print "tutorial-formatter running on", absfilename
file_ = open( absfilename, 'r' )
text_to_process = ""
current_block = ""
in_code = False
in_text = False
in_tutorial = False
for line in file_:
if not in_tutorial:
begin_pos = line.find( 'BEGIN_TUTORIAL' )
if begin_pos != -1:
text_tag = line[:begin_pos].lstrip()
tag_len = len( text_tag )
in_tutorial = True
continue
if line.find( 'END_TUTORIAL' ) != -1:
break
stripped = line.lstrip()
if stripped.startswith( text_tag.strip() ):
if in_code:
text_to_process += code_prefix + current_block + code_suffix
current_block = ""
in_code = False
in_text = True
addition = stripped[tag_len:]
if addition == '' or addition[-1] != '\n':
addition += '\n'
current_block += addition
else:
if in_text:
text_to_process += current_block
current_block = ""
in_text = False
in_code = True # Code to show begins right after tagged text
if in_code:
current_block += ' ' + line
if in_code:
text_to_process += code_prefix + current_block + code_suffix
elif in_text:
text_to_process += current_block
# Debug writes...
#print 'text_to_process ='
#print text_to_process
#print '= text_to_process'
lines = string2lines( text_to_process )
self.state_machine.insert_input( lines, absfilename )
return []
def setup(app):
app.add_directive('tutorial-formatter', TutorialFormatterDirective)

60
librviz_tutorial/src/main.cpp Executable file
View File

@ -0,0 +1,60 @@
/*
* Copyright (c) 2012, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * 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.
* * Neither the name of the Willow Garage, Inc. 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 OWNER 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.
*/
// BEGIN_TUTORIAL
// The main() for this "myviz" example is very simple, it just
// initializes ROS, creates a QApplication, creates the top-level
// widget (of type "MyViz"), shows it, and runs the Qt event loop.
#include <QApplication>
#include <ros/ros.h>
#include "myviz.h"
#include<iostream>
int main(int argc, char **argv)
{
if( !ros::isInitialized() )
{
ros::init( argc, argv, "myviz", ros::init_options::AnonymousName );
}
// std::cout << ros::init_options::AnonymousName << std::endl;
QApplication app( argc, argv );
MyViz* myviz = new MyViz();
// 设置位置和大小
myviz->setGeometry(0, 0, 1350, 1000); // 2000 1000
myviz->show();
app.exec();
delete myviz;
}

320
librviz_tutorial/src/myviz.cpp Executable file
View File

@ -0,0 +1,320 @@
#include <QColor>
#include <QSlider>
#include<QSplitter>
#include <QLabel>
#include<QPlainTextEdit>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include<QLineEdit>
#include<QSpinBox>
#include<QComboBox>
#include <QTreeWidget>
#include <QTreeWidgetItem>
#include <QHeaderView>
#include "rviz/visualization_manager.h"
#include "rviz/render_panel.h"
#include "rviz/display.h"
#include "myviz.h"
#include "../tools/color.h"
// BEGIN_TUTORIAL
// Constructor for MyViz. This does most of the work of the class.
MyViz::MyViz( QWidget* parent)
: QWidget( parent )
{
nh=ros::NodeHandle("~");
sub=nh.subscribe("/test",1000,&MyViz::subCallback,this);
pub=nh.advertise<std_msgs::String>("/test_pub",1000);
pub_thread=std::thread(&MyViz::pubThread,this);
// 全局变量存放处
// 颜色名称，可添加，待设置rgb类型设置颜色
color_name = {"yellow","red","green","black","blue","white","pink","purple","skyblue","lightgreen","orange",
};
//pub_thread.join();
// Construct and lay out labels and slider controls.
// Line Thickness
QLabel* thickness_label = new QLabel( "Line Thickness" );
QSlider* thickness_slider = new QSlider( Qt::Horizontal );
thickness_slider->setMinimum( 1 );
thickness_slider->setMaximum( 100 );
// Cell Size
QLabel* cell_size_label = new QLabel( "Cell Size" );
QSlider* cell_size_slider = new QSlider( Qt::Horizontal );
cell_size_slider->setMinimum( 1 );
cell_size_slider->setMaximum( 100 );
// Topic_Cloud
// QLabel* Topic=new QLabel("TOPIC:");
// QLineEdit* Topic_text=new QLineEdit();
// Topic_Laser 输入雷达
QLabel* Topic_laser = new QLabel("TOPIC_Laser:");
QLineEdit* Topic_laser_text = new QLineEdit();
// Laser
QLabel* Size=new QLabel("Laser Size");
QSpinBox* size=new QSpinBox();
// 网格结构
QGridLayout* controls_layout = new QGridLayout();
controls_layout->addWidget( thickness_label, 0, 0 );
controls_layout->addWidget( thickness_slider, 1, 0 );
controls_layout->addWidget( cell_size_label, 2, 0 );
controls_layout->addWidget( cell_size_slider, 3, 0 );
// controls_layout->addWidget(Topic,0,4);
// controls_layout->addWidget(Topic_text,0,5);
controls_layout->addWidget(Topic_laser, 4, 0);
controls_layout->addWidget(Topic_laser_text, 5, 0);
controls_layout->addWidget(Size,6,0);
controls_layout->addWidget(size,7,0);
// 颜色选择QComBox
QLabel* label_color = new QLabel("Color:");
QComboBox * comboBox = new QComboBox();
for(int i =0 ; i < color_name.size(); ++i){
comboBox->addItem(color_name[i]);
}
controls_layout->addWidget(label_color,8,0);
controls_layout->addWidget(comboBox,8,1);
// Tree
Tree_Display(controls_layout, 9);
// 设置网格某一行伸缩比例
controls_layout->setRowStretch(9, 6);
// controls_layout->setRowStretch(3, 0);
// Construct and lay out render panel.
render_panel_ = new rviz::RenderPanel(); // RVIZ在QT上显示的类
QHBoxLayout* main_layout = new QHBoxLayout; // 主要layout QV垂直，QH水平
main_layout->addLayout( controls_layout );
main_layout->addWidget( render_panel_,2); // 存放rviz的位置, stretch 拉伸系数
// QSplitter *splitter = new QSplitter();
// main_layout->addWidget(splitter);
// splitter->addWidget(render_panel_);
// Set the top-level layout for this MyViz widget.
setLayout( main_layout );
// Make signal/slot connections. 进行信号/插槽连接
connect( thickness_slider, SIGNAL( valueChanged( int )), this, SLOT( setThickness( int )));
connect( cell_size_slider, SIGNAL( valueChanged( int )), this, SLOT( setCellSize( int )));
// 增加雷达话题
connect(Topic_laser_text, SIGNAL(textChanged(const QString &)), this, SLOT(setLaserTopic(const QString &)));
connect(size,SIGNAL(valueChanged(int)),this,SLOT(setLaserSize(int)));
// 增加颜色变换
connect(comboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(LaserColorChanged(int)));
// Next we initialize the main RViz classes.
//
// The VisualizationManager is the container for Display objects,
// holds the main Ogre scene, holds the ViewController, etc. It is
// very central and we will probably need one in every usage of
// librviz.
manager_ = new rviz::VisualizationManager( render_panel_ ); // 是实现rviz功能的类
// 初始化camera 这行代码实现放大 缩小 平移等操作
render_panel_->initialize( manager_->getSceneManager(), manager_ );
// manager_->setFixedFrame("world");
manager_->initialize();
manager_->startUpdate();
// Create a Grid display.
grid_ = manager_->createDisplay( "rviz/Grid", "adjustable grid", true );
ROS_ASSERT( grid_ != NULL );
// cloud = manager_->createDisplay( "rviz/PointCloud2", "point cloud", true );
// ROS_ASSERT( cloud != NULL );
laser = manager_->createDisplay( "rviz/LaserScan", "Qlaser", true );
ROS_ASSERT( laser );
// 新增雷达
laser->subProp("Topic")->setValue("/scan");
laser->subProp("Size (Pixels)")->setValue("2");
laser->subProp("Color Transformer")->setValue("FlatColor");
laser->subProp("Color")->setValue(comboBox->currentText());
// cloud->subProp("Topic")->setValue("/pose_graph/octree");
// cloud->subProp("Style")->setValue("Points");
// cloud->subProp("Size (Pixels)")->setValue("2");
// cloud->subProp("Color Transformer")->setValue("Intensity");
// cloud->subProp("Invert Rainbow")->setValue("true");
// cloud->subProp("Decay Time")->setValue("1");
// Configure the GridDisplay the way we like it.
grid_->subProp( "Line Style" )->setValue( "Billboards" );
grid_->subProp( "Color" )->setValue( QColor( Qt::white ) );
// Initialize the slider values.
thickness_slider->setValue( 5 );
cell_size_slider->setValue( 6 );
}
void MyViz::Tree_Display(QGridLayout* controls_layout, int index){
QTreeWidget *menu = new QTreeWidget();
// menu->setColumnCount(2);
// 列头自适应大小
menu->header()->setSectionResizeMode(QHeaderView::ResizeToContents);
menu->setHeaderLabels(QStringList()<<"key"<<"value");
// menu->setColumnCount(2);
//设置不同层次菜单的缩进
// menu->setIndentation(10);
// global-一层树
QTreeWidgetItem *global= new QTreeWidgetItem(menu, QStringList("全局变量"));
menu->addTopLevelItem(global);
global->setExpanded(true);
//fix frame
QTreeWidgetItem *fixed_frame = new QTreeWidgetItem(global, QStringList("Fixed Frame"));
QLineEdit* fixed_frame_text = new QLineEdit();
fixed_frame_text->setText("world");
fixed_frame_text->setStyleSheet("background:transparent;border-width:0;border-style:inset");
// 设置事件
connect(fixed_frame_text,SIGNAL(textChanged(const QString&)),this,SLOT(setFixedFrame(const QString&)));
// 将其他控件(非TreeWidgetItem)放入treeItem中
menu->setItemWidget(fixed_frame, 1, fixed_frame_text); // 将container 放到 Item ,数字为列数
QTreeWidgetItem *bg_color = new QTreeWidgetItem(global, QStringList("BackGround Color"));
QLineEdit* bg_color_text = new QLineEdit();
bg_color_text->setText("48;48;48");
bg_color_text->setStyleSheet("border-width:0;border-style:outset");
// 设置事件
connect(bg_color_text,SIGNAL(textChanged(const QString&)),this,SLOT(setBackgroundColor(const QString&)));
menu->setItemWidget(bg_color, 1, bg_color_text); // 将container 放到 Item ,数字为列数
// global->addChild(fixed_frame);
// global->setTextAlignment(1, 2); // (列数，对齐方式)
// grid - 一层树
QTreeWidgetItem * grid = new QTreeWidgetItem(menu, QStringList("Grid"));
// 二层
QTreeWidgetItem *grid_color = new QTreeWidgetItem(grid, QStringList("Color"));
QComboBox * grid_color_cbox = new QComboBox();
for(int i =0 ; i < color_name.size(); ++i){
grid_color_cbox->addItem(color_name[i]);
}
connect(grid_color_cbox, SIGNAL(currentIndexChanged(int)), this, SLOT(GridColorChanged(int)));
menu->setItemWidget(grid_color, 1, grid_color_cbox);
QTreeWidgetItem *item3 = new QTreeWidgetItem(menu, QStringList("Laser"));
QTreeWidgetItem *item13 = new QTreeWidgetItem(global, QStringList("Frame rate"));
QTreeWidgetItem *item21 = new QTreeWidgetItem(grid, QStringList("Cell Size"));
QTreeWidgetItem *item22 = new QTreeWidgetItem(grid, QStringList("Line Style"));
QTreeWidgetItem *item23 = new QTreeWidgetItem(grid, QStringList("Color"));
controls_layout->addWidget(menu, index, 0 );
}
void MyViz::setBackgroundColor(const QString& color_string){
if(manager_!=NULL){
const QString t = color_string;
QColor new_color = parseColor(t);
if(new_color.isValid()){
// rviz源码中修改背景颜色的方式
render_panel_->setBackgroundColor(qtToOgre(new_color));
manager_->queueRender();
}
else
{
ROS_INFO_STREAM("Color invalid !!!");
}
}else{
ROS_INFO_STREAM("manager_ failed !!!");
}
}
void MyViz::LaserColorChanged(int index){
if(laser!=NULL) laser->subProp("Color")->setValue(color_name[index]);
}
void MyViz::GridColorChanged(int index){
if(grid_!=NULL) grid_->subProp("Color")->setValue(color_name[index]);
}
// Destructor.
MyViz::~MyViz()
{
delete manager_;
}
// This function is a Qt slot connected to a QSlider's valueChanged()
// signal. It sets the line thickness of the grid by changing the
// grid's "Line Width" property.
void MyViz::setThickness( int thickness_percent )
{
if( grid_ != NULL )
{
grid_->subProp( "Line Style" )->subProp( "Line Width" )->setValue( thickness_percent / 100.0f );
}
}
// This function is a Qt slot connected to a QSlider's valueChanged()
// signal. It sets the cell size of the grid by changing the grid's
// "Cell Size" Property.
void MyViz::setCellSize( int cell_size_percent )
{
if( grid_ != NULL )
{
grid_->subProp( "Cell Size" )->setValue( cell_size_percent / 10.0f );
}
}
void MyViz::setCloudTopic(const QString &newTopic){
if(cloud!=NULL){
cloud->subProp( "Topic" )->setValue(newTopic);
//ROS_INFO_STREAM("cloud topic changed to => "<<newTopic.toStdString());
}
}
void MyViz::setLaserTopic(const QString &newTopic){
if(laser!=NULL){
laser->subProp( "Topic" )->setValue(newTopic);
ROS_INFO_STREAM("laser topic changed to => "<<newTopic.toStdString());
}
laser->setEnabled(true);
// laser->subProp( "Color" )->setValue( QColor( Qt::yellow ) );
manager_->setFixedFrame("laser");
manager_->startUpdate();
}
void MyViz::setFixedFrame(const QString & fix_frame){
manager_->setFixedFrame(fix_frame);
manager_->startUpdate();
}
void MyViz::setCloudSize(int cloudsize){
if(cloud!=NULL){
cloud->subProp("Size (Pixels)")->setValue(cloudsize);
}
}
void MyViz::setLaserSize(int lasersize){
if(laser!=NULL){
laser->subProp("Size (Pixels)")->setValue(lasersize);
}
}
void MyViz::subCallback(const std_msgs::String& msg){
ROS_INFO_STREAM("receive message!");
}
void MyViz::pubThread(){
while(ros::ok()){
ROS_INFO_STREAM_ONCE("here is in publish process!");
}
}

81
librviz_tutorial/src/myviz.h Executable file
View File

@ -0,0 +1,81 @@
/*
* Copyright (c) 2012, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * 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.
* * Neither the name of the Willow Garage, Inc. 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 OWNER 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.
*/
#ifndef MYVIZ_H
#define MYVIZ_H
#include <QWidget>
#include<QString>
#include <QGridLayout>
#include<ros/ros.h>
#include"std_msgs/String.h"
#include<thread>
namespace rviz
{
class Display;
class RenderPanel;
class VisualizationManager;
}
// BEGIN_TUTORIAL
// Class "MyViz" implements the top level widget for this example.
class MyViz: public QWidget
{
Q_OBJECT
public:
MyViz( QWidget* parent = 0 );
virtual ~MyViz();
void subCallback(const std_msgs::String& msg);
void pubThread();
private Q_SLOTS: // QT信号和槽
void setThickness( int thickness_percent );
void setCellSize( int cell_size_percent );
void setCloudTopic(const QString &newTopic);
void setLaserTopic(const QString &newTopic);
void setCloudSize(int cloudsize);
void setLaserSize(int lasersize);
void LaserColorChanged(int index); // 雷达颜色
void GridColorChanged(int index); // 格栅颜色
void setFixedFrame(const QString &FixedFrame);
void setBackgroundColor(const QString& bg_color); // 背景颜色
void Tree_Display(QGridLayout* layout, int index);
private:
rviz::VisualizationManager* manager_; //
rviz::RenderPanel* render_panel_;
rviz::Display* grid_;
rviz::Display* cloud;
rviz::Display* laser;
ros::NodeHandle nh;
ros::Subscriber sub;
ros::Publisher pub;
std::thread pub_thread;
std::vector<QString> color_name; // 颜色的名称集合
};
// END_TUTORIAL
#endif // MYVIZ_H

360
librviz_tutorial/src/node.cpp Executable file
View File

@ -0,0 +1,360 @@
/*
* RPLIDAR ROS NODE
*
* Copyright (c) 2009 - 2014 RoboPeak Team
* http://www.robopeak.com
* Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* 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.
*
* 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.
*
*/
#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "std_srvs/Empty.h"
#include "rplidar.h"
#ifndef _countof
#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
#endif
#define DEG2RAD(x) ((x)*M_PI/180.)
using namespace rp::standalone::rplidar;
RPlidarDriver * drv = NULL;
void publish_scan(ros::Publisher *pub,
rplidar_response_measurement_node_hq_t *nodes,
size_t node_count, ros::Time start,
double scan_time, bool inverted,
float angle_min, float angle_max,
float max_distance,
std::string frame_id)
{
static int scan_count = 0;
sensor_msgs::LaserScan scan_msg;
scan_msg.header.stamp = start;
scan_msg.header.frame_id = frame_id;
scan_count++;
bool reversed = (angle_max > angle_min);
if ( reversed ) {
scan_msg.angle_min = M_PI - angle_max;
scan_msg.angle_max = M_PI - angle_min;
} else {
scan_msg.angle_min = M_PI - angle_min;
scan_msg.angle_max = M_PI - angle_max;
}
scan_msg.angle_increment =
(scan_msg.angle_max - scan_msg.angle_min) / (double)(node_count-1);
scan_msg.scan_time = scan_time;
scan_msg.time_increment = scan_time / (double)(node_count-1);
scan_msg.range_min = 0.15;
scan_msg.range_max = max_distance;//8.0;
scan_msg.intensities.resize(node_count);
scan_msg.ranges.resize(node_count);
bool reverse_data = (!inverted && reversed) || (inverted && !reversed);
if (!reverse_data) {
for (size_t i = 0; i < node_count; i++) {
float read_value = (float) nodes[i].dist_mm_q2/4.0f/1000;
if (read_value == 0.0)
scan_msg.ranges[i] = std::numeric_limits<float>::infinity();
else
scan_msg.ranges[i] = read_value;
scan_msg.intensities[i] = (float) (nodes[i].quality >> 2);
}
} else {
for (size_t i = 0; i < node_count; i++) {
float read_value = (float)nodes[i].dist_mm_q2/4.0f/1000;
if (read_value == 0.0)
scan_msg.ranges[node_count-1-i] = std::numeric_limits<float>::infinity();
else
scan_msg.ranges[node_count-1-i] = read_value;
scan_msg.intensities[node_count-1-i] = (float) (nodes[i].quality >> 2);
}
}
pub->publish(scan_msg);
}
bool getRPLIDARDeviceInfo(RPlidarDriver * drv)
{
u_result op_result;
rplidar_response_device_info_t devinfo;
op_result = drv->getDeviceInfo(devinfo);
if (IS_FAIL(op_result)) {
if (op_result == RESULT_OPERATION_TIMEOUT) {
ROS_ERROR("Error, operation time out. RESULT_OPERATION_TIMEOUT! ");
} else {
ROS_ERROR("Error, unexpected error, code: %x",op_result);
}
return false;
}
// print out the device serial number, firmware and hardware version number..
printf("RPLIDAR S/N: ");
for (int pos = 0; pos < 16 ;++pos) {
printf("%02X", devinfo.serialnum[pos]);
}
printf("\n");
ROS_INFO("Firmware Ver: %d.%02d",devinfo.firmware_version>>8, devinfo.firmware_version & 0xFF);
ROS_INFO("Hardware Rev: %d",(int)devinfo.hardware_version);
return true;
}
bool checkRPLIDARHealth(RPlidarDriver * drv)
{
u_result op_result;
rplidar_response_device_health_t healthinfo;
op_result = drv->getHealth(healthinfo);
if (IS_OK(op_result)) {
ROS_INFO("RPLidar health status : %d", healthinfo.status);
if (healthinfo.status == RPLIDAR_STATUS_ERROR) {
ROS_ERROR("Error, rplidar internal error detected. Please reboot the device to retry.");
return false;
} else {
return true;
}
} else {
ROS_ERROR("Error, cannot retrieve rplidar health code: %x", op_result);
return false;
}
}
bool stop_motor(std_srvs::Empty::Request &req,
std_srvs::Empty::Response &res)
{
if(!drv)
return false;
ROS_DEBUG("Stop motor");
drv->stop();
drv->stopMotor();
return true;
}
bool start_motor(std_srvs::Empty::Request &req,
std_srvs::Empty::Response &res)
{
if(!drv)
return false;
ROS_DEBUG("Start motor");
drv->startMotor();
drv->startScan(0,1);
return true;
}
static float getAngle(const rplidar_response_measurement_node_hq_t& node)
{
return node.angle_z_q14 * 90.f / 16384.f;
}
int main(int argc, char * argv[]) {
ros::init(argc, argv, "rplidar_node");
std::string serial_port;
int serial_baudrate = 115200;
std::string frame_id;
bool inverted = false;
bool angle_compensate = true;
float max_distance = 8.0;
int angle_compensate_multiple = 1;//it stand of angle compensate at per 1 degree
std::string scan_mode;
ros::NodeHandle nh;
ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1000);
ros::NodeHandle nh_private("~");
nh_private.param<std::string>("serial_port", serial_port, "/dev/ttyUSB0");
nh_private.param<int>("serial_baudrate", serial_baudrate, 115200/*256000*/);//ros run for A1 A2, change to 256000 if A3
nh_private.param<std::string>("frame_id", frame_id, "laser_frame");
nh_private.param<bool>("inverted", inverted, false);
nh_private.param<bool>("angle_compensate", angle_compensate, false);
nh_private.param<std::string>("scan_mode", scan_mode, std::string());
ROS_INFO("RPLIDAR running on ROS package rplidar_ros. SDK Version:"RPLIDAR_SDK_VERSION"");
u_result op_result;
// create the driver instance
drv = RPlidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_SERIALPORT);
if (!drv) {
ROS_ERROR("Create Driver fail, exit");
return -2;
}
// make connection...
if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) {
ROS_ERROR("Error, cannot bind to the specified serial port %s.",serial_port.c_str());
RPlidarDriver::DisposeDriver(drv);
return -1;
}
// get rplidar device info
if (!getRPLIDARDeviceInfo(drv)) {
return -1;
}
// check health...
if (!checkRPLIDARHealth(drv)) {
RPlidarDriver::DisposeDriver(drv);
return -1;
}
ros::ServiceServer stop_motor_service = nh.advertiseService("stop_motor", stop_motor);
ros::ServiceServer start_motor_service = nh.advertiseService("start_motor", start_motor);
drv->startMotor();
RplidarScanMode current_scan_mode;
if (scan_mode.empty()) {
op_result = drv->startScan(false /* not force scan */, true /* use typical scan mode */, 0, &current_scan_mode);
} else {
std::vector<RplidarScanMode> allSupportedScanModes;
op_result = drv->getAllSupportedScanModes(allSupportedScanModes);
if (IS_OK(op_result)) {
_u16 selectedScanMode = _u16(-1);
for (std::vector<RplidarScanMode>::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) {
if (iter->scan_mode == scan_mode) {
selectedScanMode = iter->id;
break;
}
}
if (selectedScanMode == _u16(-1)) {
ROS_ERROR("scan mode `%s' is not supported by lidar, supported modes:", scan_mode.c_str());
for (std::vector<RplidarScanMode>::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) {
ROS_ERROR("\t%s: max_distance: %.1f m, Point number: %.1fK", iter->scan_mode,
iter->max_distance, (1000/iter->us_per_sample));
}
op_result = RESULT_OPERATION_FAIL;
} else {
op_result = drv->startScanExpress(false /* not force scan */, selectedScanMode, 0, &current_scan_mode);
}
}
}
if(IS_OK(op_result))
{
//default frequent is 10 hz (by motor pwm value), current_scan_mode.us_per_sample is the number of scan point per us
angle_compensate_multiple = (int)(1000*1000/current_scan_mode.us_per_sample/10.0/360.0);
if(angle_compensate_multiple < 1)
angle_compensate_multiple = 1;
max_distance = current_scan_mode.max_distance;
ROS_INFO("current scan mode: %s, max_distance: %.1f m, Point number: %.1fK , angle_compensate: %d", current_scan_mode.scan_mode,
current_scan_mode.max_distance, (1000/current_scan_mode.us_per_sample), angle_compensate_multiple);
}
else
{
ROS_ERROR("Can not start scan: %08x!", op_result);
}
ros::Time start_scan_time;
ros::Time end_scan_time;
double scan_duration;
while (ros::ok()) {
rplidar_response_measurement_node_hq_t nodes[360*8];
size_t count = _countof(nodes);
start_scan_time = ros::Time::now();
op_result = drv->grabScanDataHq(nodes, count);
end_scan_time = ros::Time::now();
scan_duration = (end_scan_time - start_scan_time).toSec();
if (op_result == RESULT_OK) {
op_result = drv->ascendScanData(nodes, count);
float angle_min = DEG2RAD(0.0f);
float angle_max = DEG2RAD(359.0f);
if (op_result == RESULT_OK) {
if (angle_compensate) {
//const int angle_compensate_multiple = 1;
const int angle_compensate_nodes_count = 360*angle_compensate_multiple;
int angle_compensate_offset = 0;
rplidar_response_measurement_node_hq_t angle_compensate_nodes[angle_compensate_nodes_count];
memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(rplidar_response_measurement_node_hq_t));
int i = 0, j = 0;
for( ; i < count; i++ ) {
if (nodes[i].dist_mm_q2 != 0) {
float angle = getAngle(nodes[i]);
int angle_value = (int)(angle * angle_compensate_multiple);
if ((angle_value - angle_compensate_offset) < 0) angle_compensate_offset = angle_value;
for (j = 0; j < angle_compensate_multiple; j++) {
angle_compensate_nodes[angle_value-angle_compensate_offset+j] = nodes[i];
}
}
}
publish_scan(&scan_pub, angle_compensate_nodes, angle_compensate_nodes_count,
start_scan_time, scan_duration, inverted,
angle_min, angle_max, max_distance,
frame_id);
} else {
int start_node = 0, end_node = 0;
int i = 0;
// find the first valid node and last valid node
while (nodes[i++].dist_mm_q2 == 0);
start_node = i-1;
i = count -1;
while (nodes[i--].dist_mm_q2 == 0);
end_node = i+1;
angle_min = DEG2RAD(getAngle(nodes[start_node]));
angle_max = DEG2RAD(getAngle(nodes[end_node]));
publish_scan(&scan_pub, &nodes[start_node], end_node-start_node +1,
start_scan_time, scan_duration, inverted,
angle_min, angle_max, max_distance,
frame_id);
}
} else if (op_result == RESULT_OPERATION_FAIL) {
// All the data is invalid, just publish them
float angle_min = DEG2RAD(0.0f);
float angle_max = DEG2RAD(359.0f);
publish_scan(&scan_pub, nodes, count,
start_scan_time, scan_duration, inverted,
angle_min, angle_max, max_distance,
frame_id);
}
}
ros::spinOnce();
}
// done!
drv->stop();
drv->stopMotor();
RPlidarDriver::DisposeDriver(drv);
return 0;
}

52
librviz_tutorial/tools/color.cpp Normal file
View File

@ -0,0 +1,52 @@
#include"color.h"
static int limit(int i)
{
if (i < 0)
return 0;
if (i > 255)
return 255;
return i;
}
// 处理颜色
QColor parseColor(const QString& color_string)
{
if (color_string.indexOf(';') != -1)
{
QStringList strings = color_string.split(';');
if (strings.size() >= 3)
{
bool r_ok = true;
int r = strings[0].toInt(&r_ok);
bool g_ok = true;
int g = strings[1].toInt(&g_ok);
bool b_ok = true;
int b = strings[2].toInt(&b_ok);
if (r_ok && g_ok && b_ok)
{
return QColor(limit(r), limit(g), limit(b));
}
}
return QColor();
}
QColor new_color;
if (QColor::colorNames().contains(color_string, Qt::CaseInsensitive) ||
(color_string.size() > 0 && color_string[0] == '#'))
{
new_color.setNamedColor(color_string.toLower());
}
return new_color;
}
QString printColor(const QColor& color)
{
return QString("%1; %2; %3").arg(color.red()).arg(color.green()).arg(color.blue());
}
Ogre::ColourValue qtToOgre(const QColor& c)
{
return Ogre::ColourValue(c.redF(), c.greenF(), c.blueF(), c.alphaF());
}

14
librviz_tutorial/tools/color.h Normal file
View File

@ -0,0 +1,14 @@
#ifndef COLOR_H
#define COLOR_H
#include <QColor>
#include <QString>
#include <OgreColourValue.h>
// 处理颜色。将颜色字符转为Qcolor类型颜色
QColor parseColor(const QString& color_string);
QString printColor(const QColor& color);
Ogre::ColourValue qtToOgre(const QColor& qt_color);
#endif // COLOR_H