diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 120000 index 0000000..66dd650 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1 @@ +/opt/ros/melodic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/librviz_tutorial/CHANGELOG.rst b/librviz_tutorial/CHANGELOG.rst new file mode 100755 index 0000000..27ebe37 --- /dev/null +++ b/librviz_tutorial/CHANGELOG.rst @@ -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 `_) +* 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 `_) +* 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 diff --git a/librviz_tutorial/CMakeLists.txt b/librviz_tutorial/CMakeLists.txt new file mode 100755 index 0000000..e654dea --- /dev/null +++ b/librviz_tutorial/CMakeLists.txt @@ -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 +) \ No newline at end of file diff --git a/librviz_tutorial/launch/laser.launch b/librviz_tutorial/launch/laser.launch new file mode 100644 index 0000000..0a19978 --- /dev/null +++ b/librviz_tutorial/launch/laser.launch @@ -0,0 +1,10 @@ + + + + + + \ No newline at end of file diff --git a/librviz_tutorial/launch/rplidar.launch b/librviz_tutorial/launch/rplidar.launch new file mode 100755 index 0000000..0b2c7e2 --- /dev/null +++ b/librviz_tutorial/launch/rplidar.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/librviz_tutorial/package.xml b/librviz_tutorial/package.xml new file mode 100755 index 0000000..b6183b9 --- /dev/null +++ b/librviz_tutorial/package.xml @@ -0,0 +1,30 @@ + + librviz_tutorial + 0.11.0 + + Tutorial showing how to compile your own C++ program with RViz displays and features. + + Mabel Zhang + BSD + + http://ros.org/wiki/librviz_tutorial + Dave Hershberger + + catkin + + qtbase5-dev + std_msgs + roscpp + rviz + + libqt5-core + libqt5-gui + libqt5-widgets + roscpp + rviz + std_msgs + + + + + diff --git a/librviz_tutorial/rosdoc.yaml b/librviz_tutorial/rosdoc.yaml new file mode 100755 index 0000000..5b332fa --- /dev/null +++ b/librviz_tutorial/rosdoc.yaml @@ -0,0 +1,2 @@ +- builder: sphinx + sphinx_root_dir: src/doc diff --git a/librviz_tutorial/sdk/README.txt b/librviz_tutorial/sdk/README.txt new file mode 100755 index 0000000..2db6754 --- /dev/null +++ b/librviz_tutorial/sdk/README.txt @@ -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 diff --git a/librviz_tutorial/sdk/include/rplidar.h b/librviz_tutorial/sdk/include/rplidar.h new file mode 100755 index 0000000..d1c6372 --- /dev/null +++ b/librviz_tutorial/sdk/include/rplidar.h @@ -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 +#include "hal/types.h" +#include "rplidar_protocol.h" +#include "rplidar_cmd.h" + +#include "rplidar_driver.h" + +#define RPLIDAR_SDK_VERSION "1.9.0" diff --git a/librviz_tutorial/sdk/include/rplidar_cmd.h b/librviz_tutorial/sdk/include/rplidar_cmd.h new file mode 100755 index 0000000..9134e11 --- /dev/null +++ b/librviz_tutorial/sdk/include/rplidar_cmd.h @@ -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 diff --git a/librviz_tutorial/sdk/include/rplidar_driver.h b/librviz_tutorial/sdk/include/rplidar_driver.h new file mode 100755 index 0000000..20b3aee --- /dev/null +++ b/librviz_tutorial/sdk/include/rplidar_driver.h @@ -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& 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; +}; + + + + +}}} diff --git a/librviz_tutorial/sdk/include/rplidar_protocol.h b/librviz_tutorial/sdk/include/rplidar_protocol.h new file mode 100755 index 0000000..0065620 --- /dev/null +++ b/librviz_tutorial/sdk/include/rplidar_protocol.h @@ -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 diff --git a/librviz_tutorial/sdk/include/rptypes.h b/librviz_tutorial/sdk/include/rptypes.h new file mode 100755 index 0000000..88d8713 --- /dev/null +++ b/librviz_tutorial/sdk/include/rptypes.h @@ -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 + +#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 * ); diff --git a/librviz_tutorial/sdk/src/arch/linux/arch_linux.h b/librviz_tutorial/sdk/src/arch/linux/arch_linux.h new file mode 100755 index 0000000..89da485 --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/linux/arch_linux.h @@ -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 +#include +#include +#include +#include +#include +#include +#include + +// libc++ dep +#include +#include + +// linux specific +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "timer.h" + diff --git a/librviz_tutorial/sdk/src/arch/linux/net_serial.cpp b/librviz_tutorial/sdk/src/arch/linux/net_serial.cpp new file mode 100755 index 0000000..7a8736d --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/linux/net_serial.cpp @@ -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 +#include +#include +#include +#include +// linux specific + +#include +#include + +#include +#include "hal/types.h" +#include "arch/linux/net_serial.h" +#include + +#include +//__GNUC__ +#if defined(__GNUC__) +// for Linux extension +#include +#include +#include +extern "C" int tcflush(int fildes, int queue_selector); +#else +// for other standard UNIX +#include +#include + +#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(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 diff --git a/librviz_tutorial/sdk/src/arch/linux/net_serial.h b/librviz_tutorial/sdk/src/arch/linux/net_serial.h new file mode 100755 index 0000000..10ced8d --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/linux/net_serial.h @@ -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; +}; + +}}} diff --git a/librviz_tutorial/sdk/src/arch/linux/net_socket.cpp b/librviz_tutorial/sdk/src/arch/linux/net_socket.cpp new file mode 100755 index 0000000..6f77795 --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/linux/net_socket.cpp @@ -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 +#include +#include +#include +#include + +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(new sockaddr_storage); + memset(_platform_data, 0, sizeof(sockaddr_storage)); + + reinterpret_cast(_platform_data)->ss_family = AF_INET; +} + +SocketAddress::SocketAddress(const SocketAddress & src) +{ + _platform_data = reinterpret_cast(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(new sockaddr_storage); + memset(_platform_data, 0, sizeof(sockaddr_storage)); + + // default to ipv4 in case the following operation fails + reinterpret_cast(_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(_platform_data); +} + +SocketAddress::address_type_t SocketAddress::getAddressType() const +{ + switch(reinterpret_cast(_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(_platform_data)->sin_port); + case ADDRESS_TYPE_INET6: + return (int)ntohs(reinterpret_cast(_platform_data)->sin6_port); + default: + return 0; + } +} + +u_result SocketAddress::setPort(int port) +{ + switch (getAddressType()) { + case ADDRESS_TYPE_INET: + reinterpret_cast(_platform_data)->sin_port = htons((short)port); + break; + case ADDRESS_TYPE_INET6: + reinterpret_cast(_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(_platform_data)->ss_family = AF_INET; + ans = inet_pton(AF_INET, + address_string, + &reinterpret_cast(_platform_data)->sin_addr); + break; + + + case ADDRESS_TYPE_INET6: + + reinterpret_cast(_platform_data)->ss_family = AF_INET6; + ans = inet_pton(AF_INET6, + address_string, + &reinterpret_cast(_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(_platform_data)->ss_family; + const char *ans = NULL; + switch (net_family) { + case AF_INET: + ans = inet_ntop(net_family, &reinterpret_cast(_platform_data)->sin_addr, + buffer, buffersize); + break; + + case AF_INET6: + ans = inet_ntop(net_family, &reinterpret_cast(_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 &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(_platform_data)->sin_addr.s_addr, sizeof(reinterpret_cast(_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(_platform_data)->sin6_addr.s6_addr, sizeof(reinterpret_cast(_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(_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(_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(_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(_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(_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(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( const_cast(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(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(const_cast(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(const_cast(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(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(const_cast((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(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(const_cast(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(const_cast(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(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(new rp::arch::net::DGramSocketImpl(socket_fd)); + return newborn; + +} + + +}} + diff --git a/librviz_tutorial/sdk/src/arch/linux/thread.hpp b/librviz_tutorial/sdk/src/arch/linux/thread.hpp new file mode 100755 index 0000000..e978aec --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/linux/thread.hpp @@ -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 + +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, ¤t_policy, ¤t_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, ¤t_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, ¤t_policy, ¤t_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; +} + +}} diff --git a/librviz_tutorial/sdk/src/arch/linux/timer.cpp b/librviz_tutorial/sdk/src/arch/linux/timer.cpp new file mode 100755 index 0000000..926aa1e --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/linux/timer.cpp @@ -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; +} +}} diff --git a/librviz_tutorial/sdk/src/arch/linux/timer.h b/librviz_tutorial/sdk/src/arch/linux/timer.h new file mode 100755 index 0000000..abd968f --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/linux/timer.h @@ -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 +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() diff --git a/librviz_tutorial/sdk/src/arch/macOS/arch_macOS.h b/librviz_tutorial/sdk/src/arch/macOS/arch_macOS.h new file mode 100755 index 0000000..b6eb7af --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/macOS/arch_macOS.h @@ -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 +#include +#include +#include +#include +#include +#include +#include + +// libc++ dep +#include +#include + + +// POSIX specific +extern "C" { +#include +#include +#include +#include +#include +#include +#include +#include +#include +} + +#include "arch/macOS/timer.h" + diff --git a/librviz_tutorial/sdk/src/arch/macOS/net_serial.cpp b/librviz_tutorial/sdk/src/arch/macOS/net_serial.cpp new file mode 100755 index 0000000..8c83d1c --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/macOS/net_serial.cpp @@ -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 +#include + +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= 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 diff --git a/librviz_tutorial/sdk/src/arch/macOS/net_serial.h b/librviz_tutorial/sdk/src/arch/macOS/net_serial.h new file mode 100755 index 0000000..0761106 --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/macOS/net_serial.h @@ -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; +}; + +}}} diff --git a/librviz_tutorial/sdk/src/arch/macOS/net_socket.cpp b/librviz_tutorial/sdk/src/arch/macOS/net_socket.cpp new file mode 100755 index 0000000..1b6ea06 --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/macOS/net_socket.cpp @@ -0,0 +1,858 @@ +/* + * RoboPeak Project + * HAL Layer - Socket Interface + * Copyright 2018 RoboPeak Project + * + * macOS Implementation + */ + + +#include "sdkcommon.h" +#include "../../hal/socket.h" + +#include +#include +#include +#include +#include + +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(new sockaddr_storage); + memset(_platform_data, 0, sizeof(sockaddr_storage)); + + reinterpret_cast(_platform_data)->ss_family = AF_INET; +} + +SocketAddress::SocketAddress(const SocketAddress & src) +{ + _platform_data = reinterpret_cast(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(new sockaddr_storage); + memset(_platform_data, 0, sizeof(sockaddr_storage)); + + // default to ipv4 in case the following operation fails + reinterpret_cast(_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(_platform_data); +} + +SocketAddress::address_type_t SocketAddress::getAddressType() const +{ + switch(reinterpret_cast(_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(_platform_data)->sin_port); + case ADDRESS_TYPE_INET6: + return (int)ntohs(reinterpret_cast(_platform_data)->sin6_port); + default: + return 0; + } +} + +u_result SocketAddress::setPort(int port) +{ + switch (getAddressType()) { + case ADDRESS_TYPE_INET: + reinterpret_cast(_platform_data)->sin_port = htons((short)port); + break; + case ADDRESS_TYPE_INET6: + reinterpret_cast(_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(_platform_data)->ss_family = AF_INET; + ans = inet_pton(AF_INET, + address_string, + &reinterpret_cast(_platform_data)->sin_addr); + break; + + + case ADDRESS_TYPE_INET6: + + reinterpret_cast(_platform_data)->ss_family = AF_INET6; + ans = inet_pton(AF_INET6, + address_string, + &reinterpret_cast(_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(_platform_data)->ss_family; + const char *ans = NULL; + switch (net_family) { + case AF_INET: + ans = inet_ntop(net_family, &reinterpret_cast(_platform_data)->sin_addr, + buffer, buffersize); + break; + + case AF_INET6: + ans = inet_ntop(net_family, &reinterpret_cast(_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 &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(_platform_data)->sin_addr.s_addr, sizeof(reinterpret_cast(_platform_data)->sin_addr.s_addr)); + + + break; + case ADDRESS_TYPE_INET6: + if (bufferSize < sizeof(in6_addr)) return RESULT_INSUFFICIENT_MEMORY; + memcpy(buffer, reinterpret_cast(_platform_data)->sin6_addr.s6_addr, sizeof(reinterpret_cast(_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(_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(_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(_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(_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(_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(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( const_cast(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(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(const_cast(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(const_cast(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(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(const_cast((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(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(const_cast(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(const_cast(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(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(new rp::arch::net::DGramSocketImpl(socket_fd)); + return newborn; + +} + + +}} + diff --git a/librviz_tutorial/sdk/src/arch/macOS/thread.hpp b/librviz_tutorial/sdk/src/arch/macOS/thread.hpp new file mode 100755 index 0000000..f4ff330 --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/macOS/thread.hpp @@ -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; +} + +}} diff --git a/librviz_tutorial/sdk/src/arch/macOS/timer.cpp b/librviz_tutorial/sdk/src/arch/macOS/timer.cpp new file mode 100755 index 0000000..c8eac68 --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/macOS/timer.cpp @@ -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; +} + +}} diff --git a/librviz_tutorial/sdk/src/arch/macOS/timer.h b/librviz_tutorial/sdk/src/arch/macOS/timer.h new file mode 100755 index 0000000..e32ad4d --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/macOS/timer.h @@ -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 +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() diff --git a/librviz_tutorial/sdk/src/arch/win32/arch_win32.h b/librviz_tutorial/sdk/src/arch/win32/arch_win32.h new file mode 100755 index 0000000..ad5e542 --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/win32/arch_win32.h @@ -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 +#include +#include +#include //for memcpy etc.. +#include +#include + + +#include "timer.h" diff --git a/librviz_tutorial/sdk/src/arch/win32/net_serial.cpp b/librviz_tutorial/sdk/src/arch/win32/net_serial.cpp new file mode 100755 index 0000000..6c85efe --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/win32/net_serial.cpp @@ -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 diff --git a/librviz_tutorial/sdk/src/arch/win32/net_serial.h b/librviz_tutorial/sdk/src/arch/win32/net_serial.h new file mode 100755 index 0000000..6a8147b --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/win32/net_serial.h @@ -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; +}; + +}}} diff --git a/librviz_tutorial/sdk/src/arch/win32/net_socket.cpp b/librviz_tutorial/sdk/src/arch/win32/net_socket.cpp new file mode 100755 index 0000000..63cf2b0 --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/win32/net_socket.cpp @@ -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 +#include +#include + +#include +#include +#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(new sockaddr_storage); + memset(_platform_data, 0, sizeof(sockaddr_storage)); + + reinterpret_cast(_platform_data)->ss_family = AF_INET; +} + +SocketAddress::SocketAddress(const SocketAddress & src) +{ + _platform_data = reinterpret_cast(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(new sockaddr_storage); + memset(_platform_data, 0, sizeof(sockaddr_storage)); + + // default to ipv4 in case the following operation fails + reinterpret_cast(_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(_platform_data); +} + +SocketAddress::address_type_t SocketAddress::getAddressType() const +{ + switch(reinterpret_cast(_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(_platform_data)->sin_port); + case ADDRESS_TYPE_INET6: + return (int)ntohs(reinterpret_cast(_platform_data)->sin6_port); + default: + return 0; + } +} + +u_result SocketAddress::setPort(int port) +{ + switch (getAddressType()) { + case ADDRESS_TYPE_INET: + reinterpret_cast(_platform_data)->sin_port = htons((short)port); + break; + case ADDRESS_TYPE_INET6: + reinterpret_cast(_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(_platform_data)->ss_family = AF_INET; + ans = _inet_pton(AF_INET, + address_string, + &reinterpret_cast(_platform_data)->sin_addr); + break; + + + case ADDRESS_TYPE_INET6: + + reinterpret_cast(_platform_data)->ss_family = AF_INET6; + ans = _inet_pton(AF_INET6, + address_string, + &reinterpret_cast(_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(_platform_data)->ss_family; + const char *ans = NULL; + switch (net_family) { + case AF_INET: + ans = _inet_ntop(net_family, &reinterpret_cast(_platform_data)->sin_addr, + buffer, buffersize); + break; + + case AF_INET6: + ans = _inet_ntop(net_family, &reinterpret_cast(_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 &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(_platform_data)->sin_addr.s_addr)) return RESULT_INSUFFICIENT_MEMORY; + + memcpy(buffer, &reinterpret_cast(_platform_data)->sin_addr.s_addr, sizeof(reinterpret_cast(_platform_data)->sin_addr.s_addr)); + + + break; + case ADDRESS_TYPE_INET6: + if (bufferSize < sizeof(reinterpret_cast(_platform_data)->sin6_addr.s6_addr)) return RESULT_INSUFFICIENT_MEMORY; + memcpy(buffer, reinterpret_cast(_platform_data)->sin6_addr.s6_addr, sizeof(reinterpret_cast(_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(_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(_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(_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(_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(_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(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( const_cast(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(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(const_cast(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(const_cast(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(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(const_cast((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(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(const_cast(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(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(new rp::arch::net::DGramSocketImpl(socket_fd)); + return newborn; + +} + + +}} + diff --git a/librviz_tutorial/sdk/src/arch/win32/timer.cpp b/librviz_tutorial/sdk/src/arch/win32/timer.cpp new file mode 100755 index 0000000..ea9b80f --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/win32/timer.cpp @@ -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 +#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(¤t); + + return (_u32)(current.QuadPart/_current_freq.QuadPart); +} + +BEGIN_STATIC_CODE(timer_cailb) +{ + HPtimer_reset(); +}END_STATIC_CODE(timer_cailb) + +}} diff --git a/librviz_tutorial/sdk/src/arch/win32/timer.h b/librviz_tutorial/sdk/src/arch/win32/timer.h new file mode 100755 index 0000000..e47c0ca --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/win32/timer.h @@ -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() + diff --git a/librviz_tutorial/sdk/src/arch/win32/winthread.hpp b/librviz_tutorial/sdk/src/arch/win32/winthread.hpp new file mode 100755 index 0000000..363a1f1 --- /dev/null +++ b/librviz_tutorial/sdk/src/arch/win32/winthread.hpp @@ -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 + +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(this->_handle), -1)) + { + CloseHandle(reinterpret_cast(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(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(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_ABOVE_NORMAL) + { + return PRIORITY_HIGH; + } + else if (win_priorityTHREAD_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(this->_handle), timeout)) + { + case WAIT_OBJECT_0: + CloseHandle(reinterpret_cast(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; +} + +}} diff --git a/librviz_tutorial/sdk/src/hal/abs_rxtx.h b/librviz_tutorial/sdk/src/hal/abs_rxtx.h new file mode 100755 index 0000000..555153a --- /dev/null +++ b/librviz_tutorial/sdk/src/hal/abs_rxtx.h @@ -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; +}; + +}} + + + diff --git a/librviz_tutorial/sdk/src/hal/assert.h b/librviz_tutorial/sdk/src/hal/assert.h new file mode 100755 index 0000000..b17b626 --- /dev/null +++ b/librviz_tutorial/sdk/src/hal/assert.h @@ -0,0 +1,18 @@ +#ifndef _INFRA_HAL_ASSERT_H +#define _INFRA_HAL_ASSERT_H + +#ifdef WIN32 +#include +#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 diff --git a/librviz_tutorial/sdk/src/hal/byteops.h b/librviz_tutorial/sdk/src/hal/byteops.h new file mode 100755 index 0000000..5af3ed4 --- /dev/null +++ b/librviz_tutorial/sdk/src/hal/byteops.h @@ -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 diff --git a/librviz_tutorial/sdk/src/hal/event.h b/librviz_tutorial/sdk/src/hal/event.h new file mode 100755 index 0000000..550835c --- /dev/null +++ b/librviz_tutorial/sdk/src/hal/event.h @@ -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 +}; +}} diff --git a/librviz_tutorial/sdk/src/hal/locker.h b/librviz_tutorial/sdk/src/hal/locker.h new file mode 100755 index 0000000..4c621e5 --- /dev/null +++ b/librviz_tutorial/sdk/src/hal/locker.h @@ -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; +}; + + +}} + diff --git a/librviz_tutorial/sdk/src/hal/socket.h b/librviz_tutorial/sdk/src/hal/socket.h new file mode 100755 index 0000000..85d434a --- /dev/null +++ b/librviz_tutorial/sdk/src/hal/socket.h @@ -0,0 +1,151 @@ +/* + * RoboPeak Project + * HAL Layer - Socket Interface + * Copyright 2009 - 2013 RoboPeak Project + */ + +#pragma once + +#include + +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 &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() {} +}; + +}} diff --git a/librviz_tutorial/sdk/src/hal/thread.cpp b/librviz_tutorial/sdk/src/hal/thread.cpp new file mode 100755 index 0000000..c670643 --- /dev/null +++ b/librviz_tutorial/sdk/src/hal/thread.cpp @@ -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 + + diff --git a/librviz_tutorial/sdk/src/hal/thread.h b/librviz_tutorial/sdk/src/hal/thread.h new file mode 100755 index 0000000..af8bdb3 --- /dev/null +++ b/librviz_tutorial/sdk/src/hal/thread.h @@ -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(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 + static Thread create_member(T * pthis) + { + return create(_thread_thunk, pthis); + } + + template + static _word_size_t THREAD_PROC _thread_thunk(void * data) + { + return (static_cast(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; +}; + +}} + diff --git a/librviz_tutorial/sdk/src/hal/types.h b/librviz_tutorial/sdk/src/hal/types.h new file mode 100755 index 0000000..d7c180b --- /dev/null +++ b/librviz_tutorial/sdk/src/hal/types.h @@ -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 + +#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 diff --git a/librviz_tutorial/sdk/src/hal/util.h b/librviz_tutorial/sdk/src/hal/util.h new file mode 100755 index 0000000..4976aae --- /dev/null +++ b/librviz_tutorial/sdk/src/hal/util.h @@ -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 +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_; + diff --git a/librviz_tutorial/sdk/src/rplidar_driver.cpp b/librviz_tutorial/sdk/src/rplidar_driver.cpp new file mode 100755 index 0000000..7c18dd6 --- /dev/null +++ b/librviz_tutorial/sdk/src/rplidar_driver.cpp @@ -0,0 +1,2181 @@ +/* + * 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/abs_rxtx.h" +#include "hal/thread.h" +#include "hal/types.h" +#include "hal/assert.h" +#include "hal/locker.h" +#include "hal/socket.h" +#include "hal/event.h" +#include "rplidar_driver_impl.h" +#include "rplidar_driver_serial.h" +#include "rplidar_driver_TCP.h" + +#include + +#ifndef min +#define min(a,b) (((a) < (b)) ? (a) : (b)) +#endif + +namespace rp { namespace standalone{ namespace rplidar { + +#define DEPRECATED_WARN(fn, replacement) do { \ + static bool __shown__ = false; \ + if (!__shown__) { \ + printDeprecationWarn(fn, replacement); \ + __shown__ = true; \ + } \ + } while (0) + + static void printDeprecationWarn(const char* fn, const char* replacement) + { + fprintf(stderr, "*WARN* YOU ARE USING DEPRECATED API: %s, PLEASE MOVE TO %s\n", fn, replacement); + } + +static void convert(const rplidar_response_measurement_node_t& from, rplidar_response_measurement_node_hq_t& to) +{ + to.angle_z_q14 = (((from.angle_q6_checkbit) >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) << 8) / 90; //transfer to q14 Z-angle + to.dist_mm_q2 = from.distance_q2; + to.flag = (from.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT); // trasfer syncbit to HQ flag field + to.quality = (from.sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT; //remove the last two bits and then make quality from 0-63 to 0-255 +} + +static void convert(const rplidar_response_measurement_node_hq_t& from, rplidar_response_measurement_node_t& to) +{ + to.sync_quality = (from.flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) | ((from.quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT); + to.angle_q6_checkbit = 1 | (((from.angle_z_q14 * 90) >> 8) << RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT); + to.distance_q2 = from.dist_mm_q2 > _u16(-1) ? _u16(0) : _u16(from.dist_mm_q2); +} + +// Factory Impl +RPlidarDriver * RPlidarDriver::CreateDriver(_u32 drivertype) +{ + switch (drivertype) { + case DRIVER_TYPE_SERIALPORT: + return new RPlidarDriverSerial(); + case DRIVER_TYPE_TCP: + return new RPlidarDriverTCP(); + default: + return NULL; + } +} + + +void RPlidarDriver::DisposeDriver(RPlidarDriver * drv) +{ + delete drv; +} + + +RPlidarDriverImplCommon::RPlidarDriverImplCommon() + : _isConnected(false) + , _isScanning(false) + , _isSupportingMotorCtrl(false) +{ + _cached_scan_node_hq_count = 0; + _cached_scan_node_hq_count_for_interval_retrieve = 0; + _cached_sampleduration_std = LEGACY_SAMPLE_DURATION; + _cached_sampleduration_express = LEGACY_SAMPLE_DURATION; +} + +bool RPlidarDriverImplCommon::isConnected() +{ + return _isConnected; +} + + +u_result RPlidarDriverImplCommon::reset(_u32 timeout) +{ + u_result ans; + + { + rp::hal::AutoLocker l(_lock); + + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_RESET))) { + return ans; + } + } + return RESULT_OK; +} + +u_result RPlidarDriverImplCommon::_waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout) +{ + int recvPos = 0; + _u32 startTs = getms(); + _u8 recvBuffer[sizeof(rplidar_ans_header_t)]; + _u8 *headerBuffer = reinterpret_cast<_u8 *>(header); + _u32 waitTime; + + while ((waitTime=getms() - startTs) <= timeout) { + size_t remainSize = sizeof(rplidar_ans_header_t) - recvPos; + size_t recvSize; + + bool ans = _chanDev->waitfordata(remainSize, timeout - waitTime, &recvSize); + if(!ans) return RESULT_OPERATION_TIMEOUT; + + if(recvSize > remainSize) recvSize = remainSize; + + recvSize = _chanDev->recvdata(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + _u8 currentByte = recvBuffer[pos]; + switch (recvPos) { + case 0: + if (currentByte != RPLIDAR_ANS_SYNC_BYTE1) { + continue; + } + + break; + case 1: + if (currentByte != RPLIDAR_ANS_SYNC_BYTE2) { + recvPos = 0; + continue; + } + break; + } + headerBuffer[recvPos++] = currentByte; + + if (recvPos == sizeof(rplidar_ans_header_t)) { + return RESULT_OK; + } + } + } + + return RESULT_OPERATION_TIMEOUT; +} + + + +u_result RPlidarDriverImplCommon::getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout) +{ + u_result ans; + + if (!isConnected()) return RESULT_OPERATION_FAIL; + + _disableDataGrabbing(); + + { + rp::hal::AutoLocker l(_lock); + + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_HEALTH))) { + return ans; + } + + rplidar_ans_header_t response_header; + if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { + return ans; + } + + // verify whether we got a correct header + if (response_header.type != RPLIDAR_ANS_TYPE_DEVHEALTH) { + return RESULT_INVALID_DATA; + } + + _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); + if ( header_size < sizeof(rplidar_response_device_health_t)) { + return RESULT_INVALID_DATA; + } + + if (!_chanDev->waitfordata(header_size, timeout)) { + return RESULT_OPERATION_TIMEOUT; + } + _chanDev->recvdata(reinterpret_cast<_u8 *>(&healthinfo), sizeof(healthinfo)); + } + return RESULT_OK; +} + + + +u_result RPlidarDriverImplCommon::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout) +{ + u_result ans; + + if (!isConnected()) return RESULT_OPERATION_FAIL; + + _disableDataGrabbing(); + + { + rp::hal::AutoLocker l(_lock); + + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_INFO))) { + return ans; + } + + rplidar_ans_header_t response_header; + if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { + return ans; + } + + // verify whether we got a correct header + if (response_header.type != RPLIDAR_ANS_TYPE_DEVINFO) { + return RESULT_INVALID_DATA; + } + + _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); + if (header_size < sizeof(rplidar_response_device_info_t)) { + return RESULT_INVALID_DATA; + } + + if (!_chanDev->waitfordata(header_size, timeout)) { + return RESULT_OPERATION_TIMEOUT; + } + _chanDev->recvdata(reinterpret_cast<_u8 *>(&info), sizeof(info)); + } + return RESULT_OK; +} + +u_result RPlidarDriverImplCommon::getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode) +{ + DEPRECATED_WARN("getFrequency(bool,size_t,float&,bool&)", "getFrequency(const RplidarScanMode&,size_t,float&)"); + + _u16 sample_duration = inExpressMode?_cached_sampleduration_express:_cached_sampleduration_std; + frequency = 1000000.0f/(count * sample_duration); + + if (sample_duration <= 277) { + is4kmode = true; + } else { + is4kmode = false; + } + + return RESULT_OK; +} + +u_result RPlidarDriverImplCommon::getFrequency(const RplidarScanMode& scanMode, size_t count, float & frequency) +{ + float sample_duration = scanMode.us_per_sample; + frequency = 1000000.0f / (count * sample_duration); + return RESULT_OK; +} + +u_result RPlidarDriverImplCommon::_waitNode(rplidar_response_measurement_node_t * node, _u32 timeout) +{ + int recvPos = 0; + _u32 startTs = getms(); + _u8 recvBuffer[sizeof(rplidar_response_measurement_node_t)]; + _u8 *nodeBuffer = (_u8*)node; + _u32 waitTime; + + while ((waitTime=getms() - startTs) <= timeout) { + size_t remainSize = sizeof(rplidar_response_measurement_node_t) - recvPos; + size_t recvSize; + + bool ans = _chanDev->waitfordata(remainSize, timeout-waitTime, &recvSize); + if(!ans) return RESULT_OPERATION_FAIL; + + if (recvSize > remainSize) recvSize = remainSize; + + recvSize = _chanDev->recvdata(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + _u8 currentByte = recvBuffer[pos]; + switch (recvPos) { + case 0: // expect the sync bit and its reverse in this byte + { + _u8 tmp = (currentByte>>1); + if ( (tmp ^ currentByte) & 0x1 ) { + // pass + } else { + continue; + } + + } + break; + case 1: // expect the highest bit to be 1 + { + if (currentByte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) { + // pass + } else { + recvPos = 0; + continue; + } + } + break; + } + nodeBuffer[recvPos++] = currentByte; + + if (recvPos == sizeof(rplidar_response_measurement_node_t)) { + return RESULT_OK; + } + } + } + + return RESULT_OPERATION_TIMEOUT; +} + +u_result RPlidarDriverImplCommon::_waitScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout) +{ + if (!_isConnected) { + count = 0; + return RESULT_OPERATION_FAIL; + } + + size_t recvNodeCount = 0; + _u32 startTs = getms(); + _u32 waitTime; + u_result ans; + + while ((waitTime = getms() - startTs) <= timeout && recvNodeCount < count) { + rplidar_response_measurement_node_t node; + if (IS_FAIL(ans = _waitNode(&node, timeout - waitTime))) { + return ans; + } + + nodebuffer[recvNodeCount++] = node; + + if (recvNodeCount == count) return RESULT_OK; + } + count = recvNodeCount; + return RESULT_OPERATION_TIMEOUT; +} + + +u_result RPlidarDriverImplCommon::_waitCapsuledNode(rplidar_response_capsule_measurement_nodes_t & node, _u32 timeout) +{ + int recvPos = 0; + _u32 startTs = getms(); + _u8 recvBuffer[sizeof(rplidar_response_capsule_measurement_nodes_t)]; + _u8 *nodeBuffer = (_u8*)&node; + _u32 waitTime; + + + while ((waitTime=getms() - startTs) <= timeout) { + size_t remainSize = sizeof(rplidar_response_capsule_measurement_nodes_t) - recvPos; + size_t recvSize; + + bool ans = _chanDev->waitfordata(remainSize, timeout-waitTime, &recvSize); + if(!ans) + { + return RESULT_OPERATION_TIMEOUT; + } + if (recvSize > remainSize) recvSize = remainSize; + + recvSize = _chanDev->recvdata(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + _u8 currentByte = recvBuffer[pos]; + + switch (recvPos) { + case 0: // expect the sync bit 1 + { + _u8 tmp = (currentByte>>4); + if ( tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1 ) { + // pass + } else { + _is_previous_capsuledataRdy = false; + continue; + } + + } + break; + case 1: // expect the sync bit 2 + { + _u8 tmp = (currentByte>>4); + if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2) { + // pass + } else { + recvPos = 0; + _is_previous_capsuledataRdy = false; + continue; + } + } + break; + } + nodeBuffer[recvPos++] = currentByte; + if (recvPos == sizeof(rplidar_response_capsule_measurement_nodes_t)) { + // calc the checksum ... + _u8 checksum = 0; + _u8 recvChecksum = ((node.s_checksum_1 & 0xF) | (node.s_checksum_2<<4)); + for (size_t cpos = offsetof(rplidar_response_capsule_measurement_nodes_t, start_angle_sync_q6); + cpos < sizeof(rplidar_response_capsule_measurement_nodes_t); ++cpos) + { + checksum ^= nodeBuffer[cpos]; + } + if (recvChecksum == checksum) + { + // only consider vaild if the checksum matches... + if (node.start_angle_sync_q6 & RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT) + { + // this is the first capsule frame in logic, discard the previous cached data... + _is_previous_capsuledataRdy = false; + return RESULT_OK; + } + return RESULT_OK; + } + _is_previous_capsuledataRdy = false; + return RESULT_INVALID_DATA; + } + } + } + _is_previous_capsuledataRdy = false; + return RESULT_OPERATION_TIMEOUT; +} + +u_result RPlidarDriverImplCommon::_waitUltraCapsuledNode(rplidar_response_ultra_capsule_measurement_nodes_t & node, _u32 timeout) +{ + if (!_isConnected) { + return RESULT_OPERATION_FAIL; + } + + int recvPos = 0; + _u32 startTs = getms(); + _u8 recvBuffer[sizeof(rplidar_response_ultra_capsule_measurement_nodes_t)]; + _u8 *nodeBuffer = (_u8*)&node; + _u32 waitTime; + + while ((waitTime=getms() - startTs) <= timeout) { + size_t remainSize = sizeof(rplidar_response_ultra_capsule_measurement_nodes_t) - recvPos; + size_t recvSize; + + bool ans = _chanDev->waitfordata(remainSize, timeout-waitTime, &recvSize); + if(!ans) + { + return RESULT_OPERATION_TIMEOUT; + } + if (recvSize > remainSize) recvSize = remainSize; + + recvSize = _chanDev->recvdata(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + _u8 currentByte = recvBuffer[pos]; + switch (recvPos) { + case 0: // expect the sync bit 1 + { + _u8 tmp = (currentByte>>4); + if ( tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1 ) { + // pass + } + else { + _is_previous_capsuledataRdy = false; + continue; + } + } + break; + case 1: // expect the sync bit 2 + { + _u8 tmp = (currentByte>>4); + if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2) { + // pass + } + else { + recvPos = 0; + _is_previous_capsuledataRdy = false; + continue; + } + } + break; + } + nodeBuffer[recvPos++] = currentByte; + if (recvPos == sizeof(rplidar_response_ultra_capsule_measurement_nodes_t)) { + // calc the checksum ... + _u8 checksum = 0; + _u8 recvChecksum = ((node.s_checksum_1 & 0xF) | (node.s_checksum_2 << 4)); + + for (size_t cpos = offsetof(rplidar_response_ultra_capsule_measurement_nodes_t, start_angle_sync_q6); + cpos < sizeof(rplidar_response_ultra_capsule_measurement_nodes_t); ++cpos) + { + checksum ^= nodeBuffer[cpos]; + } + + if (recvChecksum == checksum) + { + // only consider vaild if the checksum matches... + if (node.start_angle_sync_q6 & RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT) + { + // this is the first capsule frame in logic, discard the previous cached data... + _is_previous_capsuledataRdy = false; + return RESULT_OK; + } + return RESULT_OK; + } + _is_previous_capsuledataRdy = false; + return RESULT_INVALID_DATA; + } + } + } + _is_previous_capsuledataRdy = false; + return RESULT_OPERATION_TIMEOUT; +} + +u_result RPlidarDriverImplCommon::_cacheScanData() +{ + rplidar_response_measurement_node_t local_buf[128]; + size_t count = 128; + rplidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES]; + size_t scan_count = 0; + u_result ans; + memset(local_scan, 0, sizeof(local_scan)); + + _waitScanData(local_buf, count); // // always discard the first data since it may be incomplete + + while(_isScanning) + { + if (IS_FAIL(ans=_waitScanData(local_buf, count))) { + if (ans != RESULT_OPERATION_TIMEOUT) { + _isScanning = false; + return RESULT_OPERATION_FAIL; + } + } + + for (size_t pos = 0; pos < count; ++pos) + { + if (local_buf[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) + { + // only publish the data when it contains a full 360 degree scan + + if ((local_scan[0].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)) { + _lock.lock(); + memcpy(_cached_scan_node_hq_buf, local_scan, scan_count*sizeof(rplidar_response_measurement_node_hq_t)); + _cached_scan_node_hq_count = scan_count; + _dataEvt.set(); + _lock.unlock(); + } + scan_count = 0; + } + + rplidar_response_measurement_node_hq_t nodeHq; + convert(local_buf[pos], nodeHq); + local_scan[scan_count++] = nodeHq; + if (scan_count == _countof(local_scan)) scan_count-=1; // prevent overflow + + //for interval retrieve + { + rp::hal::AutoLocker l(_lock); + _cached_scan_node_hq_buf_for_interval_retrieve[_cached_scan_node_hq_count_for_interval_retrieve++] = nodeHq; + if(_cached_scan_node_hq_count_for_interval_retrieve == _countof(_cached_scan_node_hq_buf_for_interval_retrieve)) _cached_scan_node_hq_count_for_interval_retrieve-=1; // prevent overflow + } + } + } + _isScanning = false; + return RESULT_OK; +} + +u_result RPlidarDriverImplCommon::startScanNormal(bool force, _u32 timeout) +{ + u_result ans; + if (!isConnected()) return RESULT_OPERATION_FAIL; + if (_isScanning) return RESULT_ALREADY_DONE; + + stop(); //force the previous operation to stop + + { + rp::hal::AutoLocker l(_lock); + + if (IS_FAIL(ans = _sendCommand(force?RPLIDAR_CMD_FORCE_SCAN:RPLIDAR_CMD_SCAN))) { + return ans; + } + + // waiting for confirmation + rplidar_ans_header_t response_header; + if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { + return ans; + } + + // verify whether we got a correct header + if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT) { + return RESULT_INVALID_DATA; + } + + _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); + if (header_size < sizeof(rplidar_response_measurement_node_t)) { + return RESULT_INVALID_DATA; + } + + _isScanning = true; + _cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheScanData); + if (_cachethread.getHandle() == 0) { + return RESULT_OPERATION_FAIL; + } + } + return RESULT_OK; +} + +u_result RPlidarDriverImplCommon::checkExpressScanSupported(bool & support, _u32 timeout) +{ + DEPRECATED_WARN("checkExpressScanSupported(bool&,_u32)", "getAllSupportedScanModes()"); + + rplidar_response_device_info_t devinfo; + + support = false; + u_result ans = getDeviceInfo(devinfo, timeout); + + if (IS_FAIL(ans)) return ans; + + if (devinfo.firmware_version >= ((0x1<<8) | 17)) { + support = true; + rplidar_response_sample_rate_t sample_rate; + getSampleDuration_uS(sample_rate); + _cached_sampleduration_express = sample_rate.express_sample_duration_us; + _cached_sampleduration_std = sample_rate.std_sample_duration_us; + } + + return RESULT_OK; +} + +u_result RPlidarDriverImplCommon::_cacheCapsuledScanData() +{ + rplidar_response_capsule_measurement_nodes_t capsule_node; + rplidar_response_measurement_node_hq_t local_buf[128]; + size_t count = 128; + rplidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES]; + size_t scan_count = 0; + u_result ans; + memset(local_scan, 0, sizeof(local_scan)); + + _waitCapsuledNode(capsule_node); // // always discard the first data since it may be incomplete + + + + + while(_isScanning) + { + if (IS_FAIL(ans=_waitCapsuledNode(capsule_node))) { + if (ans != RESULT_OPERATION_TIMEOUT && ans != RESULT_INVALID_DATA) { + _isScanning = false; + return RESULT_OPERATION_FAIL; + } else { + // current data is invalid, do not use it. + continue; + } + } + + _capsuleToNormal(capsule_node, local_buf, count); + + for (size_t pos = 0; pos < count; ++pos) + { + if (local_buf[pos].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) + { + // only publish the data when it contains a full 360 degree scan + + if ((local_scan[0].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)) { + _lock.lock(); + memcpy(_cached_scan_node_hq_buf, local_scan, scan_count*sizeof(rplidar_response_measurement_node_hq_t)); + _cached_scan_node_hq_count = scan_count; + _dataEvt.set(); + _lock.unlock(); + } + scan_count = 0; + } + local_scan[scan_count++] = local_buf[pos]; + if (scan_count == _countof(local_scan)) scan_count-=1; // prevent overflow + + //for interval retrieve + { + rp::hal::AutoLocker l(_lock); + _cached_scan_node_hq_buf_for_interval_retrieve[_cached_scan_node_hq_count_for_interval_retrieve++] = local_buf[pos]; + if(_cached_scan_node_hq_count_for_interval_retrieve == _countof(_cached_scan_node_hq_buf_for_interval_retrieve)) _cached_scan_node_hq_count_for_interval_retrieve-=1; // prevent overflow + } + } + } + _isScanning = false; + + return RESULT_OK; +} + +u_result RPlidarDriverImplCommon::_cacheUltraCapsuledScanData() +{ + rplidar_response_ultra_capsule_measurement_nodes_t ultra_capsule_node; + rplidar_response_measurement_node_hq_t local_buf[128]; + size_t count = 128; + rplidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES]; + size_t scan_count = 0; + u_result ans; + memset(local_scan, 0, sizeof(local_scan)); + + _waitUltraCapsuledNode(ultra_capsule_node); + + while(_isScanning) + { + if (IS_FAIL(ans=_waitUltraCapsuledNode(ultra_capsule_node))) { + if (ans != RESULT_OPERATION_TIMEOUT && ans != RESULT_INVALID_DATA) { + _isScanning = false; + return RESULT_OPERATION_FAIL; + } else { + // current data is invalid, do not use it. + continue; + } + } + + _ultraCapsuleToNormal(ultra_capsule_node, local_buf, count); + + for (size_t pos = 0; pos < count; ++pos) + { + if (local_buf[pos].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) + { + // only publish the data when it contains a full 360 degree scan + + if ((local_scan[0].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)) { + _lock.lock(); + memcpy(_cached_scan_node_hq_buf, local_scan, scan_count*sizeof(rplidar_response_measurement_node_hq_t)); + _cached_scan_node_hq_count = scan_count; + _dataEvt.set(); + _lock.unlock(); + } + scan_count = 0; + } + local_scan[scan_count++] = local_buf[pos]; + if (scan_count == _countof(local_scan)) scan_count-=1; // prevent overflow + + //for interval retrieve + { + rp::hal::AutoLocker l(_lock); + _cached_scan_node_hq_buf_for_interval_retrieve[_cached_scan_node_hq_count_for_interval_retrieve++] = local_buf[pos]; + if(_cached_scan_node_hq_count_for_interval_retrieve == _countof(_cached_scan_node_hq_buf_for_interval_retrieve)) _cached_scan_node_hq_count_for_interval_retrieve-=1; // prevent overflow + } + } + } + + _isScanning = false; + + return RESULT_OK; +} + +void RPlidarDriverImplCommon::_capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount) +{ + nodeCount = 0; + if (_is_previous_capsuledataRdy) { + int diffAngle_q8; + int currentStartAngle_q8 = ((capsule.start_angle_sync_q6 & 0x7FFF)<< 2); + int prevStartAngle_q8 = ((_cached_previous_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2); + + diffAngle_q8 = (currentStartAngle_q8) - (prevStartAngle_q8); + if (prevStartAngle_q8 > currentStartAngle_q8) { + diffAngle_q8 += (360<<8); + } + + int angleInc_q16 = (diffAngle_q8 << 3); + int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); + for (size_t pos = 0; pos < _countof(_cached_previous_capsuledata.cabins); ++pos) + { + int dist_q2[2]; + int angle_q6[2]; + int syncBit[2]; + + dist_q2[0] = (_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0xFFFC); + dist_q2[1] = (_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0xFFFC); + + int angle_offset1_q3 = ( (_cached_previous_capsuledata.cabins[pos].offset_angles_q3 & 0xF) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0x3)<<4)); + int angle_offset2_q3 = ( (_cached_previous_capsuledata.cabins[pos].offset_angles_q3 >> 4) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0x3)<<4)); + + angle_q6[0] = ((currentAngle_raw_q16 - (angle_offset1_q3<<13))>>10); + syncBit[0] = (( (currentAngle_raw_q16 + angleInc_q16) % (360<<16)) < angleInc_q16 )?1:0; + currentAngle_raw_q16 += angleInc_q16; + + + angle_q6[1] = ((currentAngle_raw_q16 - (angle_offset2_q3<<13))>>10); + syncBit[1] = (( (currentAngle_raw_q16 + angleInc_q16) % (360<<16)) < angleInc_q16 )?1:0; + currentAngle_raw_q16 += angleInc_q16; + + for (int cpos = 0; cpos < 2; ++cpos) { + + if (angle_q6[cpos] < 0) angle_q6[cpos] += (360<<6); + if (angle_q6[cpos] >= (360<<6)) angle_q6[cpos] -= (360<<6); + + rplidar_response_measurement_node_hq_t node; + + node.angle_z_q14 = _u16((angle_q6[cpos] << 8) / 90); + node.flag = (syncBit[cpos] | ((!syncBit[cpos]) << 1)); + node.quality = dist_q2[cpos] ? (0x2f << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0; + node.dist_mm_q2 = dist_q2[cpos]; + + nodebuffer[nodeCount++] = node; + } + + } + } + + _cached_previous_capsuledata = capsule; + _is_previous_capsuledataRdy = true; +} + +//*******************************************HQ support******************************** +u_result RPlidarDriverImplCommon::_cacheHqScanData() +{ + rplidar_response_hq_capsule_measurement_nodes_t hq_node; + rplidar_response_measurement_node_hq_t local_buf[128]; + size_t count = 128; + rplidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES]; + size_t scan_count = 0; + u_result ans; + memset(local_scan, 0, sizeof(local_scan)); + _waitHqNode(hq_node); + while (_isScanning) { + if (IS_FAIL(ans = _waitHqNode(hq_node))) { + if (ans != RESULT_OPERATION_TIMEOUT && ans != RESULT_INVALID_DATA) { + _isScanning = false; + return RESULT_OPERATION_FAIL; + } + else { + // current data is invalid, do not use it. + continue; + } + } + + _HqToNormal(hq_node, local_buf, count); + for (size_t pos = 0; pos < count; ++pos) + { + if (local_buf[pos].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) + { + // only publish the data when it contains a full 360 degree scan + if ((local_scan[0].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)) { + _lock.lock(); + memcpy(_cached_scan_node_hq_buf, local_scan, scan_count * sizeof(rplidar_response_measurement_node_hq_t)); + _cached_scan_node_hq_count = scan_count; + _dataEvt.set(); + _lock.unlock(); + } + scan_count = 0; + } + local_scan[scan_count++] = local_buf[pos]; + if (scan_count == _countof(local_scan)) scan_count -= 1; // prevent overflow + //for interval retrieve + { + rp::hal::AutoLocker l(_lock); + _cached_scan_node_hq_buf_for_interval_retrieve[_cached_scan_node_hq_count_for_interval_retrieve++] = local_buf[pos]; + if (_cached_scan_node_hq_count_for_interval_retrieve == _countof(_cached_scan_node_hq_buf_for_interval_retrieve)) _cached_scan_node_hq_count_for_interval_retrieve -= 1; // prevent overflow + } + } + + } + return RESULT_OK; +} + +//CRC calculate +static _u32 table[256];//crc32_table + +//reflect +static _u32 _bitrev(_u32 input, _u16 bw) +{ + _u16 i; + _u32 var; + var = 0; + for (i = 0; i>= 1; + } + return var; +} + +// crc32_table init +static void _crc32_init(_u32 poly) +{ + _u16 i; + _u16 j; + _u32 c; + + poly = _bitrev(poly, 32); + for (i = 0; i<256; i++){ + c = i; + for (j = 0; j<8; j++){ + if (c & 1) + c = poly ^ (c >> 1); + else + c = c >> 1; + } + table[i] = c; + } +} + +static _u32 _crc32cal(_u32 crc, void* input, _u16 len) +{ + _u16 i; + _u8 index; + _u8* pch; + pch = (unsigned char*)input; + _u8 leftBytes = 4 - len & 0x3; + + for (i = 0; i> 8) ^ table[index]; + pch++; + } + + for (i = 0; i < leftBytes; i++) {//zero padding + index = (unsigned char)(crc^0); + crc = (crc >> 8) ^ table[index]; + } + return crc^0xffffffff; +} + +//crc32cal +static u_result _crc32(_u8 *ptr, _u32 len) { + static _u8 tmp; + if (tmp != 1) { + _crc32_init(0x4C11DB7); + tmp = 1; + } + + return _crc32cal(0xFFFFFFFF, ptr,len); +} + +u_result RPlidarDriverImplCommon::_waitHqNode(rplidar_response_hq_capsule_measurement_nodes_t & node, _u32 timeout) +{ + if (!_isConnected) { + return RESULT_OPERATION_FAIL; + } + + int recvPos = 0; + _u32 startTs = getms(); + _u8 recvBuffer[sizeof(rplidar_response_hq_capsule_measurement_nodes_t)]; + _u8 *nodeBuffer = (_u8*)&node; + _u32 waitTime; + + while ((waitTime=getms() - startTs) <= timeout) { + size_t remainSize = sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - recvPos; + size_t recvSize; + + bool ans = _chanDev->waitfordata(remainSize, timeout-waitTime, &recvSize); + if(!ans) + { + return RESULT_OPERATION_TIMEOUT; + } + if (recvSize > remainSize) recvSize = remainSize; + + recvSize = _chanDev->recvdata(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + _u8 currentByte = recvBuffer[pos]; + switch (recvPos) { + case 0: // expect the sync byte + { + _u8 tmp = (currentByte); + if ( tmp == RPLIDAR_RESP_MEASUREMENT_HQ_SYNC ) { + // pass + } + else { + recvPos = 0; + _is_previous_HqdataRdy = false; + continue; + } + } + break; + case sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1 - 4: + { + + } + break; + case sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1: + { + + } + break; + } + nodeBuffer[recvPos++] = currentByte; + if (recvPos == sizeof(rplidar_response_hq_capsule_measurement_nodes_t)) { + _u32 crcCalc2 = _crc32(nodeBuffer, sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 4); + + if(crcCalc2 == node.crc32){ + _is_previous_HqdataRdy = true; + return RESULT_OK; + } + else { + _is_previous_HqdataRdy = false; + return RESULT_INVALID_DATA; + } + + } + } + } + _is_previous_HqdataRdy = false; + return RESULT_OPERATION_TIMEOUT; +} + +void RPlidarDriverImplCommon::_HqToNormal(const rplidar_response_hq_capsule_measurement_nodes_t & node_hq, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount) +{ + nodeCount = 0; + if (_is_previous_HqdataRdy) { + for (size_t pos = 0; pos < _countof(_cached_previous_Hqdata.node_hq); ++pos) + { + nodebuffer[nodeCount++] = node_hq.node_hq[pos]; + } + } + _cached_previous_Hqdata = node_hq; + _is_previous_HqdataRdy = true; + +} +//*******************************************HQ support********************************// + +static _u32 _varbitscale_decode(_u32 scaled, _u32 & scaleLevel) +{ + static const _u32 VBS_SCALED_BASE[] = { + RPLIDAR_VARBITSCALE_X16_DEST_VAL, + RPLIDAR_VARBITSCALE_X8_DEST_VAL, + RPLIDAR_VARBITSCALE_X4_DEST_VAL, + RPLIDAR_VARBITSCALE_X2_DEST_VAL, + 0, + }; + + static const _u32 VBS_SCALED_LVL[] = { + 4, + 3, + 2, + 1, + 0, + }; + + static const _u32 VBS_TARGET_BASE[] = { + (0x1 << RPLIDAR_VARBITSCALE_X16_SRC_BIT), + (0x1 << RPLIDAR_VARBITSCALE_X8_SRC_BIT), + (0x1 << RPLIDAR_VARBITSCALE_X4_SRC_BIT), + (0x1 << RPLIDAR_VARBITSCALE_X2_SRC_BIT), + 0, + }; + + for (size_t i = 0; i < _countof(VBS_SCALED_BASE); ++i) + { + int remain = ((int)scaled - (int)VBS_SCALED_BASE[i]); + if (remain >= 0) { + scaleLevel = VBS_SCALED_LVL[i]; + return VBS_TARGET_BASE[i] + (remain << scaleLevel); + } + } + return 0; +} + +void RPlidarDriverImplCommon::_ultraCapsuleToNormal(const rplidar_response_ultra_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount) +{ + nodeCount = 0; + if (_is_previous_capsuledataRdy) { + int diffAngle_q8; + int currentStartAngle_q8 = ((capsule.start_angle_sync_q6 & 0x7FFF) << 2); + int prevStartAngle_q8 = ((_cached_previous_ultracapsuledata.start_angle_sync_q6 & 0x7FFF) << 2); + + diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8); + if (prevStartAngle_q8 > currentStartAngle_q8) { + diffAngle_q8 += (360 << 8); + } + + int angleInc_q16 = (diffAngle_q8 << 3) / 3; + int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); + for (size_t pos = 0; pos < _countof(_cached_previous_ultracapsuledata.ultra_cabins); ++pos) + { + int dist_q2[3]; + int angle_q6[3]; + int syncBit[3]; + + + _u32 combined_x3 = _cached_previous_ultracapsuledata.ultra_cabins[pos].combined_x3; + + // unpack ... + int dist_major = (combined_x3 & 0xFFF); + + // signed partical integer, using the magic shift here + // DO NOT TOUCH + + int dist_predict1 = (((int)(combined_x3 << 10)) >> 22); + int dist_predict2 = (((int)combined_x3) >> 22); + + int dist_major2; + + _u32 scalelvl1, scalelvl2; + + // prefetch next ... + if (pos == _countof(_cached_previous_ultracapsuledata.ultra_cabins) - 1) + { + dist_major2 = (capsule.ultra_cabins[0].combined_x3 & 0xFFF); + } + else { + dist_major2 = (_cached_previous_ultracapsuledata.ultra_cabins[pos + 1].combined_x3 & 0xFFF); + } + + // decode with the var bit scale ... + dist_major = _varbitscale_decode(dist_major, scalelvl1); + dist_major2 = _varbitscale_decode(dist_major2, scalelvl2); + + + int dist_base1 = dist_major; + int dist_base2 = dist_major2; + + if ((!dist_major) && dist_major2) { + dist_base1 = dist_major2; + scalelvl1 = scalelvl2; + } + + + dist_q2[0] = (dist_major << 2); + if ((dist_predict1 == 0xFFFFFE00) || (dist_predict1 == 0x1FF)) { + dist_q2[1] = 0; + } else { + dist_predict1 = (dist_predict1 << scalelvl1); + dist_q2[1] = (dist_predict1 + dist_base1) << 2; + + } + + if ((dist_predict2 == 0xFFFFFE00) || (dist_predict2 == 0x1FF)) { + dist_q2[2] = 0; + } else { + dist_predict2 = (dist_predict2 << scalelvl2); + dist_q2[2] = (dist_predict2 + dist_base2) << 2; + } + + + for (int cpos = 0; cpos < 3; ++cpos) + { + + syncBit[cpos] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0; + + int offsetAngleMean_q16 = (int)(7.5 * 3.1415926535 * (1 << 16) / 180.0); + + if (dist_q2[cpos] >= (50 * 4)) + { + const int k1 = 98361; + const int k2 = int(k1 / dist_q2[cpos]); + + offsetAngleMean_q16 = (int)(8 * 3.1415926535 * (1 << 16) / 180) - (k2 << 6) - (k2 * k2 * k2) / 98304; + } + + angle_q6[cpos] = ((currentAngle_raw_q16 - int(offsetAngleMean_q16 * 180 / 3.14159265)) >> 10); + currentAngle_raw_q16 += angleInc_q16; + + if (angle_q6[cpos] < 0) angle_q6[cpos] += (360 << 6); + if (angle_q6[cpos] >= (360 << 6)) angle_q6[cpos] -= (360 << 6); + + rplidar_response_measurement_node_hq_t node; + + node.flag = (syncBit[cpos] | ((!syncBit[cpos]) << 1)); + node.quality = dist_q2[cpos] ? (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0; + node.angle_z_q14 = _u16((angle_q6[cpos] << 8) / 90); + node.dist_mm_q2 = dist_q2[cpos]; + + nodebuffer[nodeCount++] = node; + } + + } + } + + _cached_previous_ultracapsuledata = capsule; + _is_previous_capsuledataRdy = true; +} + +u_result RPlidarDriverImplCommon::checkSupportConfigCommands(bool& outSupport, _u32 timeoutInMs) +{ + u_result ans; + + rplidar_response_device_info_t devinfo; + ans = getDeviceInfo(devinfo, timeoutInMs); + if (IS_FAIL(ans)) return ans; + + // if lidar firmware >= 1.24 + if (devinfo.firmware_version >= ((0x1 << 8) | 24)) { + outSupport = true; + } + return ans; +} + +u_result RPlidarDriverImplCommon::getLidarConf(_u32 type, std::vector<_u8> &outputBuf, const std::vector<_u8> &reserve, _u32 timeout) +{ + rplidar_payload_get_scan_conf_t query; + memset(&query, 0, sizeof(query)); + query.type = type; + int sizeVec = reserve.size(); + + int maxLen = sizeof(query.reserved) / sizeof(query.reserved[0]); + if (sizeVec > maxLen) sizeVec = maxLen; + + if (sizeVec > 0) + memcpy(query.reserved, &reserve[0], reserve.size()); + + u_result ans; + { + rp::hal::AutoLocker l(_lock); + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_LIDAR_CONF, &query, sizeof(query)))) { + return ans; + } + + // waiting for confirmation + rplidar_ans_header_t response_header; + if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { + return ans; + } + + // verify whether we got a correct header + if (response_header.type != RPLIDAR_ANS_TYPE_GET_LIDAR_CONF) { + return RESULT_INVALID_DATA; + } + + _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); + if (header_size < sizeof(type)) { + return RESULT_INVALID_DATA; + } + + if (!_chanDev->waitfordata(header_size, timeout)) { + return RESULT_OPERATION_TIMEOUT; + } + + std::vector<_u8> dataBuf; + dataBuf.resize(header_size); + _chanDev->recvdata(reinterpret_cast<_u8 *>(&dataBuf[0]), header_size); + + //check if returned type is same as asked type + _u32 replyType = -1; + memcpy(&replyType, &dataBuf[0], sizeof(type)); + if (replyType != type) { + return RESULT_INVALID_DATA; + } + + //copy all the payload into &outputBuf + int payLoadLen = header_size - sizeof(type); + + //do consistency check + if (payLoadLen <= 0) { + return RESULT_INVALID_DATA; + } + //copy all payLoadLen bytes to outputBuf + outputBuf.resize(payLoadLen); + memcpy(&outputBuf[0], &dataBuf[0] + sizeof(type), payLoadLen); + } + return ans; +} + +u_result RPlidarDriverImplCommon::getTypicalScanMode(_u16& outMode, _u32 timeoutInMs) +{ + u_result ans; + std::vector<_u8> answer; + bool lidarSupportConfigCmds = false; + ans = checkSupportConfigCommands(lidarSupportConfigCmds); + if (IS_FAIL(ans)) return RESULT_INVALID_DATA; + + if (lidarSupportConfigCmds) + { + ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_TYPICAL, answer, std::vector<_u8>(), timeoutInMs); + if (IS_FAIL(ans)) { + return ans; + } + if (answer.size() < sizeof(_u16)) { + return RESULT_INVALID_DATA; + } + + const _u16 *p_answer = reinterpret_cast(&answer[0]); + outMode = *p_answer; + return ans; + } + //old version of triangle lidar + else + { + outMode = RPLIDAR_CONF_SCAN_COMMAND_EXPRESS; + return ans; + } + return ans; +} + +u_result RPlidarDriverImplCommon::getLidarSampleDuration(float& sampleDurationRes, _u16 scanModeID, _u32 timeoutInMs) +{ + u_result ans; + std::vector<_u8> reserve(2); + memcpy(&reserve[0], &scanModeID, sizeof(scanModeID)); + + std::vector<_u8> answer; + ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_US_PER_SAMPLE, answer, reserve, timeoutInMs); + if (IS_FAIL(ans)) + { + return ans; + } + if (answer.size() < sizeof(_u32)) + { + return RESULT_INVALID_DATA; + } + const _u32 *result = reinterpret_cast(&answer[0]); + sampleDurationRes = (float)(*result >> 8); + return ans; +} + +u_result RPlidarDriverImplCommon::getMaxDistance(float &maxDistance, _u16 scanModeID, _u32 timeoutInMs) +{ + u_result ans; + std::vector<_u8> reserve(2); + memcpy(&reserve[0], &scanModeID, sizeof(scanModeID)); + + std::vector<_u8> answer; + ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_MAX_DISTANCE, answer, reserve, timeoutInMs); + if (IS_FAIL(ans)) + { + return ans; + } + if (answer.size() < sizeof(_u32)) + { + return RESULT_INVALID_DATA; + } + const _u32 *result = reinterpret_cast(&answer[0]); + maxDistance = (float)(*result >> 8); + return ans; +} + +u_result RPlidarDriverImplCommon::getScanModeAnsType(_u8 &ansType, _u16 scanModeID, _u32 timeoutInMs) +{ + u_result ans; + std::vector<_u8> reserve(2); + memcpy(&reserve[0], &scanModeID, sizeof(scanModeID)); + + std::vector<_u8> answer; + ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_ANS_TYPE, answer, reserve, timeoutInMs); + if (IS_FAIL(ans)) + { + return ans; + } + if (answer.size() < sizeof(_u8)) + { + return RESULT_INVALID_DATA; + } + const _u8 *result = reinterpret_cast(&answer[0]); + ansType = *result; + return ans; +} + +u_result RPlidarDriverImplCommon::getScanModeName(char* modeName, _u16 scanModeID, _u32 timeoutInMs) +{ + u_result ans; + std::vector<_u8> reserve(2); + memcpy(&reserve[0], &scanModeID, sizeof(scanModeID)); + + std::vector<_u8> answer; + ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_NAME, answer, reserve, timeoutInMs); + if (IS_FAIL(ans)) + { + return ans; + } + int len = answer.size(); + if (0 == len) return RESULT_INVALID_DATA; + memcpy(modeName, &answer[0], len); + return ans; +} + +u_result RPlidarDriverImplCommon::getAllSupportedScanModes(std::vector& outModes, _u32 timeoutInMs) +{ + u_result ans; + bool confProtocolSupported = false; + ans = checkSupportConfigCommands(confProtocolSupported); + if (IS_FAIL(ans)) return RESULT_INVALID_DATA; + + if (confProtocolSupported) + { + // 1. get scan mode count + _u16 modeCount; + ans = getScanModeCount(modeCount); + if (IS_FAIL(ans)) + { + return RESULT_INVALID_DATA; + } + // 2. for loop to get all fields of each scan mode + for (_u16 i = 0; i < modeCount; i++) + { + RplidarScanMode scanModeInfoTmp; + memset(&scanModeInfoTmp, 0, sizeof(scanModeInfoTmp)); + scanModeInfoTmp.id = i; + ans = getLidarSampleDuration(scanModeInfoTmp.us_per_sample, i); + if (IS_FAIL(ans)) + { + return RESULT_INVALID_DATA; + } + ans = getMaxDistance(scanModeInfoTmp.max_distance, i); + if (IS_FAIL(ans)) + { + return RESULT_INVALID_DATA; + } + ans = getScanModeAnsType(scanModeInfoTmp.ans_type, i); + if (IS_FAIL(ans)) + { + return RESULT_INVALID_DATA; + } + ans = getScanModeName(scanModeInfoTmp.scan_mode, i); + if (IS_FAIL(ans)) + { + return RESULT_INVALID_DATA; + } + outModes.push_back(scanModeInfoTmp); + } + return ans; + } + else + { + rplidar_response_sample_rate_t sampleRateTmp; + ans = getSampleDuration_uS(sampleRateTmp); + if (IS_FAIL(ans)) return RESULT_INVALID_DATA; + //judge if support express scan + bool ifSupportExpScan = false; + ans = checkExpressScanSupported(ifSupportExpScan); + if (IS_FAIL(ans)) return RESULT_INVALID_DATA; + + RplidarScanMode stdScanModeInfo; + stdScanModeInfo.id = RPLIDAR_CONF_SCAN_COMMAND_STD; + stdScanModeInfo.us_per_sample = sampleRateTmp.std_sample_duration_us; + stdScanModeInfo.max_distance = 16; + stdScanModeInfo.ans_type = RPLIDAR_ANS_TYPE_MEASUREMENT; + strcpy(stdScanModeInfo.scan_mode, "Standard"); + outModes.push_back(stdScanModeInfo); + if (ifSupportExpScan) + { + RplidarScanMode expScanModeInfo; + expScanModeInfo.id = RPLIDAR_CONF_SCAN_COMMAND_EXPRESS; + expScanModeInfo.us_per_sample = sampleRateTmp.express_sample_duration_us; + expScanModeInfo.max_distance = 16; + expScanModeInfo.ans_type = RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED; + strcpy(expScanModeInfo.scan_mode, "Express"); + outModes.push_back(expScanModeInfo); + } + return ans; + } + return ans; +} + +u_result RPlidarDriverImplCommon::getScanModeCount(_u16& modeCount, _u32 timeoutInMs) +{ + u_result ans; + std::vector<_u8> answer; + ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_COUNT, answer, std::vector<_u8>(), timeoutInMs); + + if (IS_FAIL(ans)) { + return ans; + } + if (answer.size() < sizeof(_u16)) { + return RESULT_INVALID_DATA; + } + const _u16 *p_answer = reinterpret_cast(&answer[0]); + modeCount = *p_answer; + + return ans; +} + + +u_result RPlidarDriverImplCommon::startScan(bool force, bool useTypicalScan, _u32 options, RplidarScanMode* outUsedScanMode) +{ + u_result ans; + + bool ifSupportLidarConf = false; + ans = checkSupportConfigCommands(ifSupportLidarConf); + if (IS_FAIL(ans)) return RESULT_INVALID_DATA; + + if (useTypicalScan) + { + //if support lidar config protocol + if (ifSupportLidarConf) + { + _u16 typicalMode; + ans = getTypicalScanMode(typicalMode); + if (IS_FAIL(ans)) return RESULT_INVALID_DATA; + + //call startScanExpress to do the job + return startScanExpress(false, typicalMode, 0, outUsedScanMode); + } + //if old version of triangle lidar + else + { + bool isExpScanSupported = false; + ans = checkExpressScanSupported(isExpScanSupported); + if (IS_FAIL(ans)) { + return ans; + } + if (isExpScanSupported) + { + return startScanExpress(false, RPLIDAR_CONF_SCAN_COMMAND_EXPRESS, 0, outUsedScanMode); + } + } + } + + // 'useTypicalScan' is false, just use normal scan mode + if(ifSupportLidarConf) + { + if(outUsedScanMode) + { + outUsedScanMode->id = RPLIDAR_CONF_SCAN_COMMAND_STD; + ans = getLidarSampleDuration(outUsedScanMode->us_per_sample, outUsedScanMode->id); + if(IS_FAIL(ans)) + { + return RESULT_INVALID_DATA; + } + + ans = getMaxDistance(outUsedScanMode->max_distance, outUsedScanMode->id); + if(IS_FAIL(ans)) + { + return RESULT_INVALID_DATA; + } + + ans = getScanModeAnsType(outUsedScanMode->ans_type, outUsedScanMode->id); + if(IS_FAIL(ans)) + { + return RESULT_INVALID_DATA; + } + + ans = getScanModeName(outUsedScanMode->scan_mode, outUsedScanMode->id); + if(IS_FAIL(ans)) + { + return RESULT_INVALID_DATA; + } + } + } + else + { + if(outUsedScanMode) + { + rplidar_response_sample_rate_t sampleRateTmp; + ans = getSampleDuration_uS(sampleRateTmp); + if(IS_FAIL(ans)) return RESULT_INVALID_DATA; + outUsedScanMode->us_per_sample = sampleRateTmp.std_sample_duration_us; + outUsedScanMode->max_distance = 16; + outUsedScanMode->ans_type = RPLIDAR_ANS_TYPE_MEASUREMENT; + strcpy(outUsedScanMode->scan_mode, "Standard"); + } + } + + return startScanNormal(force); +} + +u_result RPlidarDriverImplCommon::startScanExpress(bool force, _u16 scanMode, _u32 options, RplidarScanMode* outUsedScanMode, _u32 timeout) +{ + u_result ans; + if (!isConnected()) return RESULT_OPERATION_FAIL; + if (_isScanning) return RESULT_ALREADY_DONE; + + stop(); //force the previous operation to stop + + if (scanMode == RPLIDAR_CONF_SCAN_COMMAND_STD) + { + return startScan(force, false, 0, outUsedScanMode); + } + + + bool ifSupportLidarConf = false; + ans = checkSupportConfigCommands(ifSupportLidarConf); + if (IS_FAIL(ans)) return RESULT_INVALID_DATA; + + if (outUsedScanMode) + { + outUsedScanMode->id = scanMode; + if (ifSupportLidarConf) + { + ans = getLidarSampleDuration(outUsedScanMode->us_per_sample, outUsedScanMode->id); + if (IS_FAIL(ans)) + { + return RESULT_INVALID_DATA; + } + + ans = getMaxDistance(outUsedScanMode->max_distance, outUsedScanMode->id); + if (IS_FAIL(ans)) + { + return RESULT_INVALID_DATA; + } + + ans = getScanModeAnsType(outUsedScanMode->ans_type, outUsedScanMode->id); + if (IS_FAIL(ans)) + { + return RESULT_INVALID_DATA; + } + + ans = getScanModeName(outUsedScanMode->scan_mode, outUsedScanMode->id); + if (IS_FAIL(ans)) + { + return RESULT_INVALID_DATA; + } + } + else + { + rplidar_response_sample_rate_t sampleRateTmp; + ans = getSampleDuration_uS(sampleRateTmp); + if (IS_FAIL(ans)) return RESULT_INVALID_DATA; + + outUsedScanMode->us_per_sample = sampleRateTmp.express_sample_duration_us; + outUsedScanMode->max_distance = 16; + outUsedScanMode->ans_type = RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED; + strcpy(outUsedScanMode->scan_mode, "Express"); + } + } + + //get scan answer type to specify how to wait data + _u8 scanAnsType; + if (ifSupportLidarConf) + getScanModeAnsType(scanAnsType, scanMode); + else + scanAnsType = RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED; + + { + rp::hal::AutoLocker l(_lock); + + rplidar_payload_express_scan_t scanReq; + memset(&scanReq, 0, sizeof(scanReq)); + if (scanMode != RPLIDAR_CONF_SCAN_COMMAND_STD && scanMode != RPLIDAR_CONF_SCAN_COMMAND_EXPRESS) + scanReq.working_mode = _u8(scanMode); + scanReq.working_flags = options; + + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_EXPRESS_SCAN, &scanReq, sizeof(scanReq)))) { + return ans; + } + + // waiting for confirmation + rplidar_ans_header_t response_header; + if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { + return ans; + } + + // verify whether we got a correct header + if (response_header.type != scanAnsType) { + return RESULT_INVALID_DATA; + } + + _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); + + if (scanAnsType == RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED) + { + if (header_size < sizeof(rplidar_response_capsule_measurement_nodes_t)) { + return RESULT_INVALID_DATA; + } + _isScanning = true; + _cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheCapsuledScanData); + }else if(scanAnsType ==RPLIDAR_ANS_TYPE_MEASUREMENT_HQ ){ + if (header_size < sizeof(rplidar_response_hq_capsule_measurement_nodes_t)) { + return RESULT_INVALID_DATA; + } + _isScanning = true; + _cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheHqScanData); + + } + else + { + if (header_size < sizeof(rplidar_response_ultra_capsule_measurement_nodes_t)) { + return RESULT_INVALID_DATA; + } + _isScanning = true; + _cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheUltraCapsuledScanData); + } + + if (_cachethread.getHandle() == 0) { + return RESULT_OPERATION_FAIL; + } + } + return RESULT_OK; +} + +u_result RPlidarDriverImplCommon::stop(_u32 timeout) +{ + u_result ans; + _disableDataGrabbing(); + + { + rp::hal::AutoLocker l(_lock); + + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_STOP))) { + return ans; + } + } + + return RESULT_OK; +} + +u_result RPlidarDriverImplCommon::grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout) +{ + DEPRECATED_WARN("grabScanData()", "grabScanDataHq()"); + + switch (_dataEvt.wait(timeout)) + { + case rp::hal::Event::EVENT_TIMEOUT: + count = 0; + return RESULT_OPERATION_TIMEOUT; + case rp::hal::Event::EVENT_OK: + { + if(_cached_scan_node_hq_count == 0) return RESULT_OPERATION_TIMEOUT; //consider as timeout + + rp::hal::AutoLocker l(_lock); + + size_t size_to_copy = min(count, _cached_scan_node_hq_count); + + for (size_t i = 0; i < size_to_copy; i++) + convert(_cached_scan_node_hq_buf[i], nodebuffer[i]); + + count = size_to_copy; + _cached_scan_node_hq_count = 0; + } + return RESULT_OK; + + default: + count = 0; + return RESULT_OPERATION_FAIL; + } +} + +u_result RPlidarDriverImplCommon::grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout) +{ + switch (_dataEvt.wait(timeout)) + { + case rp::hal::Event::EVENT_TIMEOUT: + count = 0; + return RESULT_OPERATION_TIMEOUT; + case rp::hal::Event::EVENT_OK: + { + if (_cached_scan_node_hq_count == 0) return RESULT_OPERATION_TIMEOUT; //consider as timeout + + rp::hal::AutoLocker l(_lock); + + size_t size_to_copy = min(count, _cached_scan_node_hq_count); + memcpy(nodebuffer, _cached_scan_node_hq_buf, size_to_copy * sizeof(rplidar_response_measurement_node_hq_t)); + + count = size_to_copy; + _cached_scan_node_hq_count = 0; + } + return RESULT_OK; + + default: + count = 0; + return RESULT_OPERATION_FAIL; + } +} + +u_result RPlidarDriverImplCommon::getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count) +{ + DEPRECATED_WARN("getScanDataWithInterval(rplidar_response_measurement_node_t*, size_t&)", "getScanDataWithInterval(rplidar_response_measurement_node_hq_t*, size_t&)"); + + size_t size_to_copy = 0; + { + rp::hal::AutoLocker l(_lock); + if(_cached_scan_node_hq_count_for_interval_retrieve == 0) + { + return RESULT_OPERATION_TIMEOUT; + } + //copy all the nodes(_cached_scan_node_count_for_interval_retrieve nodes) in _cached_scan_node_buf_for_interval_retrieve + size_to_copy = _cached_scan_node_hq_count_for_interval_retrieve; + for (size_t i = 0; i < size_to_copy; i++) + { + convert(_cached_scan_node_hq_buf_for_interval_retrieve[i], nodebuffer[i]); + } + _cached_scan_node_hq_count_for_interval_retrieve = 0; + } + count = size_to_copy; + + return RESULT_OK; +} + +u_result RPlidarDriverImplCommon::getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count) +{ + size_t size_to_copy = 0; + { + rp::hal::AutoLocker l(_lock); + if (_cached_scan_node_hq_count_for_interval_retrieve == 0) + { + return RESULT_OPERATION_TIMEOUT; + } + //copy all the nodes(_cached_scan_node_count_for_interval_retrieve nodes) in _cached_scan_node_buf_for_interval_retrieve + size_to_copy = _cached_scan_node_hq_count_for_interval_retrieve; + memcpy(nodebuffer, _cached_scan_node_hq_buf_for_interval_retrieve, size_to_copy * sizeof(rplidar_response_measurement_node_hq_t)); + _cached_scan_node_hq_count_for_interval_retrieve = 0; + } + count = size_to_copy; + + return RESULT_OK; +} + +static inline float getAngle(const rplidar_response_measurement_node_t& node) +{ + return (node.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.f; +} + +static inline void setAngle(rplidar_response_measurement_node_t& node, float v) +{ + _u16 checkbit = node.angle_q6_checkbit & RPLIDAR_RESP_MEASUREMENT_CHECKBIT; + node.angle_q6_checkbit = (((_u16)(v * 64.0f)) << RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) | checkbit; +} + +static inline float getAngle(const rplidar_response_measurement_node_hq_t& node) +{ + return node.angle_z_q14 * 90.f / 16384.f; +} + +static inline void setAngle(rplidar_response_measurement_node_hq_t& node, float v) +{ + node.angle_z_q14 = _u32(v * 16384.f / 90.f); +} + +static inline _u16 getDistanceQ2(const rplidar_response_measurement_node_t& node) +{ + return node.distance_q2; +} + +static inline _u32 getDistanceQ2(const rplidar_response_measurement_node_hq_t& node) +{ + return node.dist_mm_q2; +} + +template +static bool angleLessThan(const TNode& a, const TNode& b) +{ + return getAngle(a) < getAngle(b); +} + +template < class TNode > +static u_result ascendScanData_(TNode * nodebuffer, size_t count) +{ + float inc_origin_angle = 360.f/count; + size_t i = 0; + + //Tune head + for (i = 0; i < count; i++) { + if(getDistanceQ2(nodebuffer[i]) == 0) { + continue; + } else { + while(i != 0) { + i--; + float expect_angle = getAngle(nodebuffer[i+1]) - inc_origin_angle; + if (expect_angle < 0.0f) expect_angle = 0.0f; + setAngle(nodebuffer[i], expect_angle); + } + break; + } + } + + // all the data is invalid + if (i == count) return RESULT_OPERATION_FAIL; + + //Tune tail + for (i = count - 1; i >= 0; i--) { + if(getDistanceQ2(nodebuffer[i]) == 0) { + continue; + } else { + while(i != (count - 1)) { + i++; + float expect_angle = getAngle(nodebuffer[i-1]) + inc_origin_angle; + if (expect_angle > 360.0f) expect_angle -= 360.0f; + setAngle(nodebuffer[i], expect_angle); + } + break; + } + } + + //Fill invalid angle in the scan + float frontAngle = getAngle(nodebuffer[0]); + for (i = 1; i < count; i++) { + if(getDistanceQ2(nodebuffer[i]) == 0) { + float expect_angle = frontAngle + i * inc_origin_angle; + if (expect_angle > 360.0f) expect_angle -= 360.0f; + setAngle(nodebuffer[i], expect_angle); + } + } + + // Reorder the scan according to the angle value + std::sort(nodebuffer, nodebuffer + count, &angleLessThan); + + return RESULT_OK; +} + +u_result RPlidarDriverImplCommon::ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count) +{ + DEPRECATED_WARN("ascendScanData(rplidar_response_measurement_node_t*, size_t)", "ascendScanData(rplidar_response_measurement_node_hq_t*, size_t)"); + + return ascendScanData_(nodebuffer, count); +} + +u_result RPlidarDriverImplCommon::ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count) +{ + return ascendScanData_(nodebuffer, count); +} + +u_result RPlidarDriverImplCommon::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize) +{ + _u8 pkt_header[10]; + rplidar_cmd_packet_t * header = reinterpret_cast(pkt_header); + _u8 checksum = 0; + + if (!_isConnected) return RESULT_OPERATION_FAIL; + + if (payloadsize && payload) { + cmd |= RPLIDAR_CMDFLAG_HAS_PAYLOAD; + } + + header->syncByte = RPLIDAR_CMD_SYNC_BYTE; + header->cmd_flag = cmd; + + // send header first + _chanDev->senddata(pkt_header, 2) ; + + if (cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) { + checksum ^= RPLIDAR_CMD_SYNC_BYTE; + checksum ^= cmd; + checksum ^= (payloadsize & 0xFF); + + // calc checksum + for (size_t pos = 0; pos < payloadsize; ++pos) { + checksum ^= ((_u8 *)payload)[pos]; + } + + // send size + _u8 sizebyte = payloadsize; + _chanDev->senddata(&sizebyte, 1); + + // send payload + _chanDev->senddata((const _u8 *)payload, sizebyte); + + // send checksum + _chanDev->senddata(&checksum, 1); + } + + return RESULT_OK; +} + +u_result RPlidarDriverImplCommon::getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout) +{ + DEPRECATED_WARN("getSampleDuration_uS", "RplidarScanMode::us_per_sample"); + + if (!isConnected()) return RESULT_OPERATION_FAIL; + + _disableDataGrabbing(); + + rplidar_response_device_info_t devinfo; + // 1. fetch the device version first... + u_result ans = getDeviceInfo(devinfo, timeout); + + rateInfo.express_sample_duration_us = _cached_sampleduration_express; + rateInfo.std_sample_duration_us = _cached_sampleduration_std; + + if (devinfo.firmware_version < ((0x1<<8) | 17)) { + // provide fake data... + + return RESULT_OK; + } + + + { + rp::hal::AutoLocker l(_lock); + + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_SAMPLERATE))) { + return ans; + } + + rplidar_ans_header_t response_header; + if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { + return ans; + } + + // verify whether we got a correct header + if (response_header.type != RPLIDAR_ANS_TYPE_SAMPLE_RATE) { + return RESULT_INVALID_DATA; + } + + _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); + if ( header_size < sizeof(rplidar_response_sample_rate_t)) { + return RESULT_INVALID_DATA; + } + + if (!_chanDev->waitfordata(header_size, timeout)) { + return RESULT_OPERATION_TIMEOUT; + } + _chanDev->recvdata(reinterpret_cast<_u8 *>(&rateInfo), sizeof(rateInfo)); + } + return RESULT_OK; +} + +u_result RPlidarDriverImplCommon::checkMotorCtrlSupport(bool & support, _u32 timeout) +{ + u_result ans; + support = false; + + if (!isConnected()) return RESULT_OPERATION_FAIL; + + _disableDataGrabbing(); + + { + rp::hal::AutoLocker l(_lock); + + rplidar_payload_acc_board_flag_t flag; + flag.reserved = 0; + + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_ACC_BOARD_FLAG, &flag, sizeof(flag)))) { + return ans; + } + + rplidar_ans_header_t response_header; + if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { + return ans; + } + + // verify whether we got a correct header + if (response_header.type != RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG) { + return RESULT_INVALID_DATA; + } + + _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); + if ( header_size < sizeof(rplidar_response_acc_board_flag_t)) { + return RESULT_INVALID_DATA; + } + + if (!_chanDev->waitfordata(header_size, timeout)) { + return RESULT_OPERATION_TIMEOUT; + } + rplidar_response_acc_board_flag_t acc_board_flag; + _chanDev->recvdata(reinterpret_cast<_u8 *>(&acc_board_flag), sizeof(acc_board_flag)); + + if (acc_board_flag.support_flag & RPLIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK) { + support = true; + } + } + return RESULT_OK; +} + +u_result RPlidarDriverImplCommon::setMotorPWM(_u16 pwm) +{ + u_result ans; + rplidar_payload_motor_pwm_t motor_pwm; + motor_pwm.pwm_value = pwm; + + { + rp::hal::AutoLocker l(_lock); + + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_SET_MOTOR_PWM,(const _u8 *)&motor_pwm, sizeof(motor_pwm)))) { + return ans; + } + } + + return RESULT_OK; +} + +u_result RPlidarDriverImplCommon::startMotor() +{ + if (_isSupportingMotorCtrl) { // RPLIDAR A2 + setMotorPWM(DEFAULT_MOTOR_PWM); + delay(500); + return RESULT_OK; + } else { // RPLIDAR A1 + rp::hal::AutoLocker l(_lock); + _chanDev->clearDTR(); + delay(500); + return RESULT_OK; + } +} + +u_result RPlidarDriverImplCommon::stopMotor() +{ + if (_isSupportingMotorCtrl) { // RPLIDAR A2 + setMotorPWM(0); + delay(500); + return RESULT_OK; + } else { // RPLIDAR A1 + rp::hal::AutoLocker l(_lock); + _chanDev->setDTR(); + delay(500); + return RESULT_OK; + } +} + +void RPlidarDriverImplCommon::_disableDataGrabbing() +{ + _isScanning = false; + _cachethread.join(); +} + +// Serial Driver Impl + +RPlidarDriverSerial::RPlidarDriverSerial() +{ + _chanDev = new SerialChannelDevice(); +} + +RPlidarDriverSerial::~RPlidarDriverSerial() +{ + // force disconnection + disconnect(); + + _chanDev->close(); + _chanDev->ReleaseRxTx(); +} + +void RPlidarDriverSerial::disconnect() +{ + if (!_isConnected) return ; + stop(); +} + +u_result RPlidarDriverSerial::connect(const char * port_path, _u32 baudrate, _u32 flag) +{ + if (isConnected()) return RESULT_ALREADY_DONE; + + if (!_chanDev) return RESULT_INSUFFICIENT_MEMORY; + + { + rp::hal::AutoLocker l(_lock); + + // establish the serial connection... + if (!_chanDev->bind(port_path, baudrate) || !_chanDev->open()) { + return RESULT_INVALID_DATA; + } + _chanDev->flush(); + } + + _isConnected = true; + + checkMotorCtrlSupport(_isSupportingMotorCtrl); + stopMotor(); + + return RESULT_OK; +} + +RPlidarDriverTCP::RPlidarDriverTCP() +{ + _chanDev = new TCPChannelDevice(); +} + +RPlidarDriverTCP::~RPlidarDriverTCP() +{ + // force disconnection + disconnect(); +} + +void RPlidarDriverTCP::disconnect() +{ + if (!_isConnected) return ; + stop(); + _chanDev->close(); +} + +u_result RPlidarDriverTCP::connect(const char * ipStr, _u32 port, _u32 flag) +{ + if (isConnected()) return RESULT_ALREADY_DONE; + + if (!_chanDev) return RESULT_INSUFFICIENT_MEMORY; + + { + rp::hal::AutoLocker l(_lock); + + // establish the serial connection... + if(!_chanDev->bind(ipStr, port)) + return RESULT_INVALID_DATA; + } + + _isConnected = true; + + checkMotorCtrlSupport(_isSupportingMotorCtrl); + stopMotor(); + + return RESULT_OK; +} + +}}} diff --git a/librviz_tutorial/sdk/src/rplidar_driver_TCP.h b/librviz_tutorial/sdk/src/rplidar_driver_TCP.h new file mode 100755 index 0000000..74bc062 --- /dev/null +++ b/librviz_tutorial/sdk/src/rplidar_driver_TCP.h @@ -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(); +}; + + +}}} \ No newline at end of file diff --git a/librviz_tutorial/sdk/src/rplidar_driver_impl.h b/librviz_tutorial/sdk/src/rplidar_driver_impl.h new file mode 100755 index 0000000..b4135fd --- /dev/null +++ b/librviz_tutorial/sdk/src/rplidar_driver_impl.h @@ -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& 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() {} +}; +}}} \ No newline at end of file diff --git a/librviz_tutorial/sdk/src/rplidar_driver_serial.h b/librviz_tutorial/sdk/src/rplidar_driver_serial.h new file mode 100755 index 0000000..c6a6ea0 --- /dev/null +++ b/librviz_tutorial/sdk/src/rplidar_driver_serial.h @@ -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(); + +}; + +}}} diff --git a/librviz_tutorial/sdk/src/sdkcommon.h b/librviz_tutorial/sdk/src/sdkcommon.h new file mode 100755 index 0000000..41de548 --- /dev/null +++ b/librviz_tutorial/sdk/src/sdkcommon.h @@ -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" \ No newline at end of file diff --git a/librviz_tutorial/src/doc/conf.py b/librviz_tutorial/src/doc/conf.py new file mode 100755 index 0000000..bb18b12 --- /dev/null +++ b/librviz_tutorial/src/doc/conf.py @@ -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', '')} diff --git a/librviz_tutorial/src/doc/index.rst b/librviz_tutorial/src/doc/index.rst new file mode 100755 index 0000000..28910ab --- /dev/null +++ b/librviz_tutorial/src/doc/index.rst @@ -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 diff --git a/librviz_tutorial/src/doc/myviz.png b/librviz_tutorial/src/doc/myviz.png new file mode 100755 index 0000000..ab623d2 Binary files /dev/null and b/librviz_tutorial/src/doc/myviz.png differ diff --git a/librviz_tutorial/src/doc/tutorialformatter.py b/librviz_tutorial/src/doc/tutorialformatter.py new file mode 100755 index 0000000..8260942 --- /dev/null +++ b/librviz_tutorial/src/doc/tutorialformatter.py @@ -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 +""" + +__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) diff --git a/librviz_tutorial/src/main.cpp b/librviz_tutorial/src/main.cpp new file mode 100755 index 0000000..c1f5026 --- /dev/null +++ b/librviz_tutorial/src/main.cpp @@ -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 +#include +#include "myviz.h" +#include + +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; +} diff --git a/librviz_tutorial/src/myviz.cpp b/librviz_tutorial/src/myviz.cpp new file mode 100755 index 0000000..b8ec1b4 --- /dev/null +++ b/librviz_tutorial/src/myviz.cpp @@ -0,0 +1,320 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#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("/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 => "<subProp( "Topic" )->setValue(newTopic); + ROS_INFO_STREAM("laser topic changed to => "<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!"); + } +} \ No newline at end of file diff --git a/librviz_tutorial/src/myviz.h b/librviz_tutorial/src/myviz.h new file mode 100755 index 0000000..3450cbb --- /dev/null +++ b/librviz_tutorial/src/myviz.h @@ -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 +#include +#include +#include +#include"std_msgs/String.h" +#include +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 color_name; // 颜色的名称集合 +}; +// END_TUTORIAL +#endif // MYVIZ_H diff --git a/librviz_tutorial/src/node.cpp b/librviz_tutorial/src/node.cpp new file mode 100755 index 0000000..df0ba90 --- /dev/null +++ b/librviz_tutorial/src/node.cpp @@ -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::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::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("scan", 1000); + ros::NodeHandle nh_private("~"); + nh_private.param("serial_port", serial_port, "/dev/ttyUSB0"); + nh_private.param("serial_baudrate", serial_baudrate, 115200/*256000*/);//ros run for A1 A2, change to 256000 if A3 + nh_private.param("frame_id", frame_id, "laser_frame"); + nh_private.param("inverted", inverted, false); + nh_private.param("angle_compensate", angle_compensate, false); + nh_private.param("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, ¤t_scan_mode); + } else { + std::vector allSupportedScanModes; + op_result = drv->getAllSupportedScanModes(allSupportedScanModes); + + if (IS_OK(op_result)) { + _u16 selectedScanMode = _u16(-1); + for (std::vector::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::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, ¤t_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; +} diff --git a/librviz_tutorial/tools/color.cpp b/librviz_tutorial/tools/color.cpp new file mode 100644 index 0000000..ef83941 --- /dev/null +++ b/librviz_tutorial/tools/color.cpp @@ -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()); +} diff --git a/librviz_tutorial/tools/color.h b/librviz_tutorial/tools/color.h new file mode 100644 index 0000000..c7f5cb8 --- /dev/null +++ b/librviz_tutorial/tools/color.h @@ -0,0 +1,14 @@ +#ifndef COLOR_H +#define COLOR_H + +#include +#include +#include + +// 处理颜色。将颜色字符转为Qcolor类型颜色 +QColor parseColor(const QString& color_string); +QString printColor(const QColor& color); + +Ogre::ColourValue qtToOgre(const QColor& qt_color); + +#endif // COLOR_H