├── rover_driver ├── launch │ ├── rover_base.yaml │ └── driver.launch ├── package.xml ├── src │ └── rover_driver_node.cpp └── CMakeLists.txt ├── frames.pdf ├── STM32-firmware ├── main.bin ├── main.elf └── main.cpp ├── rover_2dnav ├── launch │ ├── mymap.pgm │ ├── view_nav.launch │ ├── mymap.yaml │ ├── rover_robot.launch │ ├── global_costmap_params.yaml │ ├── local_costmap_params.yaml │ ├── move_base_params.yaml │ ├── base_local_planner_params.yaml │ ├── costmap_common_params.yaml │ ├── move_base.launch │ └── rover_amcl.launch ├── package.xml ├── CMakeLists.txt └── rviz │ └── nav.rviz ├── rover_robot ├── CMakeLists.txt └── package.xml ├── ydlidar ├── launch │ ├── view_lidar.launch │ ├── lidar_view.launch │ ├── lidar.launch │ └── lidar.rviz ├── sdk │ ├── src │ │ ├── impl │ │ │ ├── windows │ │ │ │ ├── win.h │ │ │ │ ├── win_timer.cpp │ │ │ │ └── win_serial.h │ │ │ ├── unix │ │ │ │ ├── unix.h │ │ │ │ ├── unix_timer.cpp │ │ │ │ └── unix_serial.h │ │ │ └── list_ports │ │ │ │ ├── list_ports_win.cpp │ │ │ │ ├── list_ports_osx.cpp │ │ │ │ └── list_ports_linux.cpp │ │ ├── common.h │ │ └── serial.cpp │ ├── include │ │ ├── utils.h │ │ ├── v8stdint.h │ │ ├── timer.h │ │ ├── thread.h │ │ ├── CYdLidar.h │ │ ├── locker.h │ │ ├── ydlidar_driver.h │ │ ├── CYdLidar.cpp │ │ └── serial.h │ ├── CMakeLists.txt │ ├── samples │ │ ├── CMakeLists.txt │ │ └── main.cpp │ └── README.md ├── startup │ └── initenv.sh ├── test │ ├── CMakeLists.txt │ ├── main.cpp │ ├── laser_test.h │ └── laser_test.cpp ├── src │ └── ydlidar_client.cpp ├── CMakeLists.txt ├── package.xml └── README.md ├── README.md ├── rover_tool ├── launch │ └── teleop.launch ├── src │ └── tele_op_node.cpp ├── package.xml └── CMakeLists.txt ├── frames.gv └── hector_slam └── launch ├── hector_mapping.launch └── default_hector_mapping.launch /rover_driver/launch/rover_base.yaml: -------------------------------------------------------------------------------- 1 | base_width: 0.16 2 | -------------------------------------------------------------------------------- /frames.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zlingkang/rover_robot/HEAD/frames.pdf -------------------------------------------------------------------------------- /STM32-firmware/main.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zlingkang/rover_robot/HEAD/STM32-firmware/main.bin -------------------------------------------------------------------------------- /STM32-firmware/main.elf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zlingkang/rover_robot/HEAD/STM32-firmware/main.elf -------------------------------------------------------------------------------- /rover_2dnav/launch/mymap.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zlingkang/rover_robot/HEAD/rover_2dnav/launch/mymap.pgm -------------------------------------------------------------------------------- /rover_robot/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rover_robot) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /ydlidar/launch/view_lidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /rover_2dnav/launch/view_nav.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /rover_2dnav/launch/mymap.yaml: -------------------------------------------------------------------------------- 1 | image: mymap.pgm 2 | resolution: 0.050000 3 | origin: [-51.224998, -51.224998, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /rover_2dnav/launch/rover_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /ydlidar/launch/lidar_view.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /ydlidar/sdk/src/impl/windows/win.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | -------------------------------------------------------------------------------- /rover_2dnav/launch/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_link 4 | update_frequency: 2.0 5 | publish_frequency: 0.1 6 | static_map: true 7 | transform_tolerance: 3.0 8 | -------------------------------------------------------------------------------- /rover_2dnav/launch/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 2.0 5 | publish_frequency: 0.5 6 | static_map: false 7 | rolling_window: true 8 | width: 3.0 9 | height: 3.0 10 | resolution: 0.05 11 | transform_tolerance: 3.0 12 | -------------------------------------------------------------------------------- /rover_2dnav/launch/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | shutdown_costmaps: false 2 | 3 | controller_frequency: 5.0 #before 5.0 4 | controller_patience: 3.0 5 | 6 | planner_frequency: 0.5 7 | planner_patience: 5.0 8 | 9 | oscillation_timeout: 10.0 10 | oscillation_distance: 0.2 11 | 12 | conservative_reset_dist: 0.10 #distance from an obstacle at which it will unstuck itself 13 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # rover_robot 2 | A low-cost ROS navigation-enabled mobile robot platform 3 | 4 | # project details 5 | [http://zlethic.com/low-cos-ros-nav/](http://zlethic.com/low-cos-ros-nav/) 6 | 7 | ![](http://i0.wp.com/zlethic.com/wp-content/uploads/2018/05/IMG_20180506_233646-3.jpg?w=945) 8 | 9 | ![](http://i1.wp.com/zlethic.com/wp-content/uploads/2018/05/IMG_20180506_233646-4.jpg?w=945) 10 | -------------------------------------------------------------------------------- /ydlidar/sdk/include/utils.h: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | #ifdef WIN32 5 | #ifdef ydlidar_EXPORTS 6 | #define YDLIDAR_API __declspec(dllexport) 7 | #else 8 | #ifdef ydlidarStatic_EXPORTS 9 | #define YDLIDAR_API 10 | #else 11 | 12 | #define YDLIDAR_API __declspec(dllimport) 13 | #endif // YDLIDAR_STATIC_EXPORTS 14 | #endif 15 | 16 | #else 17 | #define YDLIDAR_API 18 | #endif // ifdef WIN32 19 | -------------------------------------------------------------------------------- /ydlidar/sdk/src/common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #if defined(_WIN32) 4 | #include "impl\windows\win.h" 5 | #include "impl\windows\win_serial.h" 6 | #elif defined(__GNUC__) 7 | #include "impl/unix/unix.h" 8 | #include "impl/unix/unix_serial.h" 9 | #else 10 | #error "unsupported target" 11 | #endif 12 | 13 | #include "locker.h" 14 | #include "serial.h" 15 | #include "thread.h" 16 | #include "timer.h" 17 | 18 | #define SDKVerision "1.3.1" 19 | -------------------------------------------------------------------------------- /rover_2dnav/launch/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | TrajectoryPlannerROS: 2 | max_vel_x: 0.45 3 | min_vel_x: -0.45 4 | max_vel_theta: 1.0 5 | min_vel_theta: -1.0 6 | min_in_place_vel_theta: 0.4 7 | 8 | acc_lim_theta: 3.2 9 | acc_lim_x: 2.5 10 | 11 | xy_goal_tolerance: 0.25 12 | yaw_goal_tolerance: 0.25 13 | 14 | holonomic_robot: false 15 | 16 | max_vel_y: 0.0 17 | min_vel_y: 0.0 18 | acc_lim_y: 0.0 19 | vy_samples: 0 20 | -------------------------------------------------------------------------------- /rover_2dnav/launch/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 2.5 2 | raytrace_range: 3.0 3 | footprint: [[-0.1, -0.1], [-0.1, 0.1], [0.1, 0.1], [0.1, -0.1]] 4 | #robot_radius: ir_of_robot 5 | inflation_radius: 0.20 6 | transform_tolerance: 3.0 7 | 8 | observation_sources: laser_scan_sensor #point_cloud_sensor 9 | 10 | laser_scan_sensor: {sensor_frame: laser_frame, data_type: LaserScan, topic: scan, marking: true, clearing: true} 11 | 12 | #point_cloud_sensor: {sensor_frame: frame_name, data_type: PointCloud, topic: topic_name, marking: true, clearing: true} 13 | -------------------------------------------------------------------------------- /ydlidar/sdk/src/impl/unix/unix.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // libc dep 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | // libc++ dep 13 | #include 14 | #include 15 | 16 | // linux specific 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | -------------------------------------------------------------------------------- /rover_tool/launch/teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /ydlidar/startup/initenv.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0666", GROUP:="dialout", SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar.rules 3 | 4 | echo 'KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", MODE:="0666", GROUP:="dialout", SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar-V2.rules 5 | 6 | echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="067b", ATTRS{idProduct}=="2303", MODE:="0666", GROUP:="dialout", SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar-2303.rules 7 | 8 | service udev reload 9 | sleep 2 10 | service udev restart 11 | 12 | -------------------------------------------------------------------------------- /ydlidar/sdk/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | add_definitions(-std=c++11) # Use C++11 3 | include_directories(include) 4 | include_directories(src) 5 | 6 | IF (WIN32) 7 | FILE(GLOB SDK_SRC 8 | "src/*.cpp" 9 | "src/*.h" 10 | "src/impl/windows/*.cpp" 11 | "src/impl/windows/*.h" 12 | ) 13 | 14 | ELSE() 15 | FILE(GLOB SDK_SRC 16 | "src/*.cpp" 17 | "src/*.h" 18 | "src/impl/unix/*.cpp" 19 | "src/impl/unix/*.h" 20 | ) 21 | 22 | 23 | ENDIF() 24 | 25 | add_subdirectory(samples) 26 | 27 | add_library(ydlidar_driver SHARED ${SDK_SRC}) 28 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 29 | IF (WIN32) 30 | ELSE() 31 | target_link_libraries(ydlidar_driver rt pthread) 32 | ENDIF() 33 | -------------------------------------------------------------------------------- /frames.gv: -------------------------------------------------------------------------------- 1 | digraph G { 2 | "map" -> "odom"[label="Broadcaster: /amcl\nAverage rate: 7.383 Hz\nMost recent transform: 1531612875.105 ( -2.881 sec old)\nBuffer length: 3.928 sec\n"]; 3 | "odom" -> "base_link"[label="Broadcaster: /rover_driver_node\nAverage rate: 822.106 Hz\nMost recent transform: 1531612871.682 ( 0.542 sec old)\nBuffer length: 3.025 sec\n"]; 4 | "base_link" -> "laser_frame"[label="Broadcaster: /base_link_to_laser4\nAverage rate: 25.441 Hz\nMost recent transform: 1531612871.655 ( 0.569 sec old)\nBuffer length: 1.769 sec\n"]; 5 | edge [style=invis]; 6 | subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; 7 | "Recorded at time: 1531612872.224"[ shape=plaintext ] ; 8 | }->"map"; 9 | } -------------------------------------------------------------------------------- /ydlidar/sdk/src/impl/unix/unix_timer.cpp: -------------------------------------------------------------------------------- 1 | #if !defined(_WIN32) 2 | #include "timer.h" 3 | 4 | namespace impl{ 5 | uint32_t getHDTimer() { 6 | struct timespec t; 7 | t.tv_sec = t.tv_nsec = 0; 8 | clock_gettime(CLOCK_MONOTONIC, &t); 9 | return t.tv_sec*1000L + t.tv_nsec/1000000L; 10 | } 11 | TTimeStamp getCurrentTime() 12 | { 13 | #if HAS_CLOCK_GETTIME 14 | struct timespec tim; 15 | clock_gettime(CLOCK_REALTIME, &tim); 16 | return (TTimeStamp)(tim.tv_sec*1000000000LL + tim.tv_nsec); 17 | #else 18 | struct timeval timeofday; 19 | gettimeofday(&timeofday,NULL); 20 | return (TTimeStamp)( timeofday.tv_sec*1000000000LL + timeofday.tv_usec * 1000); 21 | #endif 22 | } 23 | } 24 | #endif 25 | -------------------------------------------------------------------------------- /rover_driver/launch/driver.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /ydlidar/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(laser_test) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 5 | 6 | ADD_DEFINITIONS(-std=c++11) 7 | 8 | 9 | set(SDK_PATH "../sdk/") 10 | 11 | IF (WIN32) 12 | FILE(GLOB SDK_SRC 13 | "${SDK_PATH}/src/impl/windows/*.cpp" 14 | "${SDK_PATH}/src/*.cpp" 15 | ) 16 | 17 | ELSE() 18 | 19 | FILE(GLOB SDK_SRC 20 | "${SDK_PATH}/src/impl/unix/*.cpp" 21 | "${SDK_PATH}/src/*.cpp" 22 | ) 23 | 24 | ENDIF() 25 | 26 | include_directories( 27 | ${catkin_INCLUDE_DIRS} 28 | ${SDK_PATH}//include 29 | ${SDK_PATH}/src 30 | ) 31 | 32 | set(MODULE_FILES 33 | laser_test.h 34 | laser_test.cpp 35 | ) 36 | 37 | add_executable(laser_test main.cpp ${MODULE_FILES} ${SDK_SRC}) 38 | 39 | IF (WIN32) 40 | ELSE() 41 | target_link_libraries(laser_test rt pthread) 42 | ENDIF() 43 | -------------------------------------------------------------------------------- /ydlidar/sdk/samples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | cmake_minimum_required(VERSION 2.8) 3 | PROJECT(ydlidar_test) 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 5 | add_definitions(-std=c++11) # Use C++11 6 | 7 | set(YDLIDAR_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../include) 8 | IF (WIN32) 9 | list(APPEND YDLIDAR_LIBRARIES ${CMAKE_CURRENT_SOURCE_DIR}/../lib/libydlidar_driver.dll) 10 | ELSE() 11 | list(APPEND YDLIDAR_LIBRARIES ${CMAKE_CURRENT_SOURCE_DIR}/../lib/libydlidar_driver.so) 12 | ENDIF() 13 | 14 | 15 | #Include directories 16 | INCLUDE_DIRECTORIES( 17 | ${CMAKE_SOURCE_DIR} 18 | ${CMAKE_SOURCE_DIR}/../ 19 | ${CMAKE_CURRENT_BINARY_DIR} 20 | ${YDLIDAR_INCLUDE_DIRS} 21 | ) 22 | 23 | 24 | ADD_EXECUTABLE(${PROJECT_NAME} 25 | main.cpp) 26 | 27 | # Add the required libraries for linking: 28 | TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${YDLIDAR_LIBRARIES}) 29 | -------------------------------------------------------------------------------- /ydlidar/sdk/README.md: -------------------------------------------------------------------------------- 1 | YDLIDAR SDK PACKAGE V1.3.1 2 | ===================================================================== 3 | 4 | SDK test application for YDLIDAR 5 | 6 | Visit EAI Website for more details about YDLIDAR. 7 | 8 | How to build YDLIDAR SDK samples 9 | ===================================================================== 10 | 1) Clone this project to your computer folder 11 | 2) Clone this project to your computer folder 12 | 2) Running cmake to build ydlidar_test 13 | 14 | How to run YDLIDAR SDK samples 15 | ===================================================================== 16 | --$ cd samples 17 | --$ ./ydlidar_test /dev/ttyUSB0 115200 0 18 | 19 | You should see YDLIDAR's scan result in the console 20 | 21 | 22 | Upgrade Log 23 | ===================================================================== 24 | 2018-04-16 version:1.3.1 25 | 1.Compensate for each laser point timestamp. 26 | 27 | -------------------------------------------------------------------------------- /ydlidar/sdk/src/impl/windows/win_timer.cpp: -------------------------------------------------------------------------------- 1 | #if defined(_WIN32) 2 | #include "timer.h" 3 | #include 4 | #pragma comment(lib, "Winmm.lib") 5 | 6 | namespace impl{ 7 | 8 | static LARGE_INTEGER _current_freq; 9 | 10 | void HPtimer_reset() { 11 | BOOL ans=QueryPerformanceFrequency(&_current_freq); 12 | _current_freq.QuadPart/=1000; 13 | } 14 | 15 | uint32_t getHDTimer() { 16 | LARGE_INTEGER current; 17 | QueryPerformanceCounter(¤t); 18 | 19 | return (uint32_t)(current.QuadPart/(_current_freq.QuadPart)); 20 | } 21 | 22 | TTimeStamp getCurrentTime(){ 23 | FILETIME t; 24 | GetSystemTimeAsFileTime(&t); 25 | return ((((uint64_t)t.dwHighDateTime) << 32) | ((uint64_t)t.dwLowDateTime))*100; 26 | } 27 | 28 | 29 | BEGIN_STATIC_CODE(timer_cailb) { 30 | HPtimer_reset(); 31 | }END_STATIC_CODE(timer_cailb) 32 | 33 | } 34 | #endif 35 | -------------------------------------------------------------------------------- /ydlidar/test/main.cpp: -------------------------------------------------------------------------------- 1 | #include "laser_test.h" 2 | #include 3 | #include 4 | #include 5 | using namespace std; 6 | 7 | int main(int argc, char * argv[]) 8 | { 9 | bool showHelp = argc>1 && !strcmp(argv[1],"--help"); 10 | printf(" YDLIDAR C++ TEST\n"); 11 | // Process arguments: 12 | if (argc<4 || showHelp ){ 13 | printf("Usage: %s \n\n",argv[0]); 14 | printf("Example:%s /dev/ttyUSB0 115200 0\n\n",argv[0]); 15 | if (!showHelp){ 16 | return -1; 17 | }else{ 18 | return 0; 19 | } 20 | } 21 | 22 | const std::string port = string(argv[1]); 23 | const int baud = atoi(argv[2]); 24 | const int intensities = atoi(argv[3]); 25 | 26 | Lasertest laser; 27 | laser.setPort(port); 28 | laser.setBaudrate(baud); 29 | laser.setIntensities(intensities); 30 | laser.run(); 31 | 32 | return 0; 33 | } 34 | -------------------------------------------------------------------------------- /rover_2dnav/launch/move_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /ydlidar/src/ydlidar_client.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * YDLIDAR SYSTEM 3 | * YDLIDAR ROS Node Client 4 | * 5 | * Copyright 2015 - 2018 EAI TEAM 6 | * http://www.ydlidar.com 7 | * 8 | */ 9 | 10 | #include "ros/ros.h" 11 | #include "sensor_msgs/LaserScan.h" 12 | 13 | #define RAD2DEG(x) ((x)*180./M_PI) 14 | 15 | void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan) 16 | { 17 | int count = scan->scan_time / scan->time_increment; 18 | printf("[YDLIDAR INFO]: I heard a laser scan %s[%d]:\n", scan->header.frame_id.c_str(), count); 19 | printf("[YDLIDAR INFO]: angle_range : [%f, %f]\n", RAD2DEG(scan->angle_min), RAD2DEG(scan->angle_max)); 20 | 21 | for(int i = 0; i < count; i++) { 22 | float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i); 23 | if(degree > -5 && degree< 5) 24 | printf("[YDLIDAR INFO]: angle-distance : [%f, %f, %i]\n", degree, scan->ranges[i], i); 25 | } 26 | } 27 | 28 | int main(int argc, char **argv) 29 | { 30 | ros::init(argc, argv, "ydlidar_client"); 31 | ros::NodeHandle n; 32 | ros::Subscriber sub = n.subscribe("/scan", 1000, scanCallback); 33 | ros::spin(); 34 | 35 | return 0; 36 | } 37 | -------------------------------------------------------------------------------- /ydlidar/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ydlidar) 3 | 4 | add_definitions(-std=c++11) # Use C++11 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | rosconsole 8 | roscpp 9 | sensor_msgs 10 | ) 11 | 12 | #add_subdirectory(sdk) 13 | 14 | set(SDK_PATH "./sdk/") 15 | 16 | FILE(GLOB SDK_SRC 17 | "${SDK_PATH}/src/impl/unix/*.cpp" 18 | "${SDK_PATH}/src/*.cpp" 19 | ) 20 | 21 | catkin_package() 22 | 23 | include_directories( 24 | ${catkin_INCLUDE_DIRS} 25 | ${PROJECT_SOURCE_DIR}/src 26 | ${PROJECT_SOURCE_DIR}/sdk/include 27 | ${PROJECT_SOURCE_DIR}/sdk/src 28 | ) 29 | 30 | add_executable(ydlidar_node src/ydlidar_node.cpp ${SDK_SRC}) 31 | add_executable(ydlidar_client src/ydlidar_client.cpp) 32 | 33 | target_link_libraries(ydlidar_node 34 | ${catkin_LIBRARIES} 35 | ) 36 | target_link_libraries(ydlidar_client 37 | ${catkin_LIBRARIES} 38 | ) 39 | 40 | install(TARGETS ydlidar_node ydlidar_client 41 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 42 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 43 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 44 | ) 45 | 46 | 47 | install(DIRECTORY launch startup sdk 48 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 49 | USE_SOURCE_PERMISSIONS 50 | ) 51 | 52 | 53 | -------------------------------------------------------------------------------- /ydlidar/launch/lidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /rover_tool/src/tele_op_node.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "geometry_msgs/Twist.h" 3 | #include "sensor_msgs/Joy.h" 4 | 5 | class TeleopRover 6 | { 7 | public: 8 | TeleopRover(); 9 | 10 | private: 11 | void joyCallback(const sensor_msgs::Joy::ConstPtr& joy); 12 | 13 | ros::NodeHandle nh_; 14 | 15 | int linear_, angular_; 16 | double l_scale_, a_scale_; 17 | ros::Publisher vel_pub_; 18 | ros::Subscriber joy_sub_; 19 | }; 20 | 21 | TeleopRover::TeleopRover(): 22 | linear_(1), 23 | angular_(2) 24 | { 25 | nh_.param("axis_linear", linear_, linear_); 26 | nh_.param("axis_angular", angular_, angular_); 27 | nh_.param("scale_angular", a_scale_, a_scale_); 28 | nh_.param("scale_linear", l_scale_, l_scale_); 29 | 30 | vel_pub_ = nh_.advertise("/cmd_vel", 1); 31 | 32 | joy_sub_ = nh_.subscribe("joy", 10, &TeleopRover::joyCallback, this); 33 | } 34 | 35 | void TeleopRover::joyCallback(const sensor_msgs::Joy::ConstPtr& joy) 36 | { 37 | geometry_msgs::Twist twist; 38 | twist.angular.z = a_scale_*joy->axes[angular_]; 39 | twist.linear.x = l_scale_*joy->axes[linear_]; 40 | vel_pub_.publish(twist); 41 | } 42 | 43 | int main(int argc, char *argv[]) 44 | { 45 | ros::init(argc, argv, "tele_op_node"); 46 | 47 | TeleopRover teleop_rover; 48 | 49 | ros::spin(); 50 | } 51 | -------------------------------------------------------------------------------- /ydlidar/test/laser_test.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "../sdk/include/ydlidar_driver.h" 3 | #include "../sdk/src/common.h" 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #define DEG2RAD(x) ((x)*M_PI/180.) 10 | 11 | #define RAD2DEG(x) ((x)*180./M_PI) 12 | #define NODE_COUNTS 720 13 | #define EACH_ANGLE 0.5 14 | 15 | const double PI = 3.1415926; 16 | 17 | class Lasertest { 18 | public: 19 | Lasertest(); 20 | 21 | virtual ~Lasertest(); 22 | 23 | void run(); 24 | 25 | void setPort(std::string port); 26 | void setBaudrate(int baud); 27 | void setIntensities(bool intensities); 28 | private: 29 | void Open(); 30 | void Start(); 31 | void Stop(); 32 | void Close(); 33 | static void closeApp(int signo); 34 | 35 | bool getDeviceHealth(); 36 | bool getDeviceInfo(); 37 | 38 | std::vector split(const std::string &s, char delim); 39 | 40 | void publicScanData(node_info *nodes, uint64_t start,double scan_time, size_t node_count, float angle_min, float angle_max,bool reverse_data); 41 | 42 | 43 | enum DEVICE_STATE { 44 | OPENED, 45 | RUNNING, 46 | CLOSED, 47 | }; 48 | 49 | static DEVICE_STATE device_state_; 50 | 51 | int scan_no_; 52 | 53 | std::string port_; 54 | int baudrate_; 55 | bool intensities_; 56 | int publish_freq_; 57 | double angle_min_, angle_max_; 58 | bool inverted; 59 | }; 60 | 61 | 62 | -------------------------------------------------------------------------------- /ydlidar/sdk/include/v8stdint.h: -------------------------------------------------------------------------------- 1 | #ifndef V8STDINT_H_ 2 | #define V8STDINT_H_ 3 | 4 | #include 5 | #include 6 | 7 | #if defined(_WIN32) && !defined(__MINGW32__) 8 | typedef signed char int8_t; 9 | typedef unsigned char uint8_t; 10 | typedef short int16_t; 11 | typedef unsigned short uint16_t; 12 | typedef int int32_t; 13 | typedef unsigned int uint32_t; 14 | typedef __int64 int64_t; 15 | typedef unsigned __int64 uint64_t; 16 | #else 17 | 18 | #include 19 | 20 | #endif 21 | 22 | #define __small_endian 23 | 24 | #ifndef __GNUC__ 25 | #define __attribute__(x) 26 | #endif 27 | 28 | 29 | #ifdef _AVR_ 30 | typedef uint8_t _size_t; 31 | #define THREAD_PROC 32 | #elif defined (WIN64) 33 | typedef uint64_t _size_t; 34 | #define THREAD_PROC __stdcall 35 | #elif defined (WIN32) 36 | typedef uint32_t _size_t; 37 | #define THREAD_PROC __stdcall 38 | #elif defined (_M_X64) 39 | typedef uint64_t _size_t; 40 | #define THREAD_PROC __stdcall 41 | #elif defined (__GNUC__) 42 | typedef unsigned long _size_t; 43 | #define THREAD_PROC 44 | #elif defined (__ICCARM__) 45 | typedef uint32_t _size_t; 46 | #define THREAD_PROC 47 | #endif 48 | 49 | typedef _size_t (THREAD_PROC * thread_proc_t ) ( void * ); 50 | 51 | typedef int32_t result_t; 52 | typedef uint64_t TTimeStamp; 53 | 54 | #define RESULT_OK 0 55 | #define RESULT_TIMEOUT -1 56 | #define RESULT_FAIL -2 57 | 58 | #define INVALID_TIMESTAMP (0) 59 | 60 | 61 | 62 | #endif // V8STDINT_H_ 63 | -------------------------------------------------------------------------------- /ydlidar/sdk/samples/main.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "CYdLidar.h" 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | using namespace std; 9 | using namespace ydlidar; 10 | CYdLidar laser; 11 | static bool running = false; 12 | 13 | static void Stop(int signo) 14 | { 15 | 16 | printf("Received exit signal\n"); 17 | running = true; 18 | 19 | } 20 | 21 | int main(int argc, char * argv[]) 22 | { 23 | 24 | bool showHelp = argc>1 && !strcmp(argv[1],"--help"); 25 | printf(" YDLIDAR C++ TEST\n"); 26 | if (argc<4 || showHelp ) 27 | { 28 | 29 | printf("Usage: %s \n\n",argv[0]); 30 | printf("Example:%s /dev/ttyUSB0 115200 0\n\n",argv[0]); 31 | if (!showHelp) 32 | { 33 | return -1; 34 | } 35 | else 36 | return 0; 37 | } 38 | 39 | const std::string port = string(argv[1]); 40 | const int baud = atoi(argv[2]); 41 | const int intensities = atoi(argv[3]); 42 | 43 | signal(SIGINT, Stop); 44 | signal(SIGTERM, Stop); 45 | 46 | laser.setSerialPort(port); 47 | laser.setSerialBaudrate(baud); 48 | laser.setIntensities(intensities); 49 | 50 | laser.initialize(); 51 | while(!running){ 52 | bool hardError; 53 | LaserScan scan; 54 | 55 | if(laser.doProcessSimple(scan, hardError )){ 56 | fprintf(stderr,"Scan received: %u ranges\n",(unsigned int)scan.ranges.size()); 57 | 58 | } 59 | usleep(50*1000); 60 | 61 | 62 | } 63 | laser.turnOff(); 64 | laser.disconnecting(); 65 | 66 | return 0; 67 | 68 | 69 | } 70 | -------------------------------------------------------------------------------- /rover_2dnav/launch/rover_amcl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /ydlidar/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ydlidar 4 | 1.3.1 5 | The ydlidar package 6 | 7 | 8 | 9 | 10 | shao 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | rosconsole 44 | roscpp 45 | sensor_msgs 46 | rosconsole 47 | roscpp 48 | sensor_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /ydlidar/sdk/include/timer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "v8stdint.h" 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | 9 | #define BEGIN_STATIC_CODE( _blockname_ ) \ 10 | static class _static_code_##_blockname_ { \ 11 | public: \ 12 | _static_code_##_blockname_ () 13 | 14 | 15 | #define END_STATIC_CODE( _blockname_ ) \ 16 | } _instance_##_blockname_; 17 | 18 | 19 | #if defined(_WIN32) 20 | #include 21 | #define delay(x) ::Sleep(x) 22 | #else 23 | #include 24 | #include 25 | 26 | static inline void delay(uint32_t ms){ 27 | while (ms>=1000){ 28 | usleep(1000*1000); 29 | ms-=1000; 30 | }; 31 | if (ms!=0){ 32 | usleep(ms*1000); 33 | } 34 | } 35 | #endif 36 | 37 | 38 | 39 | 40 | 41 | static inline TTimeStamp time_tToTimestamp(const time_t &t ) 42 | { 43 | return uint64_t(t) * UINT64_C(10000000) + UINT64_C(116444736) * UINT64_C(1000000000); 44 | } 45 | 46 | static inline TTimeStamp time_tToTimestamp(const double t ) 47 | { 48 | return uint64_t(t*10000000.0)+ UINT64_C(116444736)*UINT64_C(1000000000); 49 | } 50 | 51 | static inline double timestampTotime_t( const TTimeStamp t ) 52 | { 53 | return double(t - UINT64_C(116444736)*UINT64_C(1000000000)) / 10000000.0; 54 | } 55 | 56 | static inline TTimeStamp timestampAdd( const TTimeStamp tim, const double num_seconds) 57 | { 58 | return static_cast(tim + static_cast(num_seconds*10000000.0)); 59 | } 60 | 61 | /*--------------------------------------------------------------- 62 | timeDifference 63 | ---------------------------------------------------------------*/ 64 | static inline double timeDifference( const TTimeStamp t1, const TTimeStamp t2 ) 65 | { 66 | assert(t1!=INVALID_TIMESTAMP); 67 | assert(t2!=INVALID_TIMESTAMP); 68 | 69 | return (int64_t(t2)- int64_t(t1))/10000000.0; 70 | } 71 | 72 | /*--------------------------------------------------------------- 73 | secondsToTimestamp 74 | ---------------------------------------------------------------*/ 75 | static inline TTimeStamp secondsToTimestamp( const double nSeconds ) 76 | { 77 | return (TTimeStamp)(nSeconds*10000000.0); 78 | } 79 | 80 | namespace impl{ 81 | 82 | #if defined(_WIN32) 83 | void HPtimer_reset(); 84 | #endif 85 | uint32_t getHDTimer(); 86 | TTimeStamp getCurrentTime(); 87 | } 88 | 89 | 90 | #define getms() impl::getHDTimer() 91 | #define getTime() impl::getCurrentTime() 92 | -------------------------------------------------------------------------------- /hector_slam/launch/hector_mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /hector_slam/launch/default_hector_mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /rover_robot/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rover_robot 4 | 0.0.0 5 | The rover_robot package 6 | 7 | 8 | 9 | 10 | lingkang 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | rover_driver 53 | rover_tool 54 | rover_2dnav 55 | ydlidar 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /rover_2dnav/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rover_2dnav 4 | 0.0.0 5 | The rover_2dnav package 6 | 7 | 8 | 9 | 10 | lingkang 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | move_base 53 | move_base 54 | move_base 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /ydlidar/sdk/include/thread.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "v8stdint.h" 3 | 4 | #ifdef _WIN32 5 | #include 6 | #include 7 | #include 8 | #include 9 | #else 10 | #include 11 | #include 12 | #endif 13 | 14 | #define UNUSED(x) (void)x 15 | 16 | #if defined(__ANDROID__) 17 | #define pthread_cancel(x) 0 18 | #endif 19 | 20 | #define CLASS_THREAD(c , x ) Thread::ThreadCreateObjectFunctor(this) 21 | 22 | class Thread 23 | { 24 | public: 25 | 26 | template static Thread ThreadCreateObjectFunctor(CLASS * pthis){ 27 | return createThread(createThreadAux, pthis); 28 | } 29 | 30 | template static _size_t THREAD_PROC createThreadAux(void * param){ 31 | return (static_cast(param)->*PROC)(); 32 | } 33 | 34 | static Thread createThread(thread_proc_t proc, void * param = NULL ){ 35 | Thread thread_(proc, param); 36 | #if defined(_WIN32) 37 | thread_._handle = (_size_t)( _beginthreadex(NULL, 0, (unsigned int (__stdcall * )( void * ))proc, param, 0, NULL)); 38 | #else 39 | assert( sizeof(thread_._handle) >= sizeof(pthread_t)); 40 | 41 | pthread_create((pthread_t *)&thread_._handle, NULL, (void * (*)(void *))proc, param); 42 | #endif 43 | return thread_; 44 | } 45 | 46 | public: 47 | explicit Thread(): _param(NULL),_func(NULL),_handle(0){} 48 | virtual ~Thread(){} 49 | _size_t getHandle(){ 50 | return _handle; 51 | } 52 | int terminate(){ 53 | #if defined(_WIN32) 54 | if (!this->_handle){ 55 | return 0; 56 | } 57 | if (TerminateThread( reinterpret_cast(this->_handle), -1)){ 58 | CloseHandle(reinterpret_cast(this->_handle)); 59 | this->_handle = NULL; 60 | return 0; 61 | }else{ 62 | return -2; 63 | } 64 | 65 | #else 66 | if (!this->_handle) return 0; 67 | 68 | return pthread_cancel((pthread_t)this->_handle); 69 | #endif 70 | } 71 | void *getParam() { 72 | return _param; 73 | } 74 | int join(unsigned long timeout = -1){ 75 | if (!this->_handle){ 76 | return 0; 77 | } 78 | #if defined(_WIN32) 79 | switch ( WaitForSingleObject(reinterpret_cast(this->_handle), timeout)){ 80 | case WAIT_OBJECT_0: 81 | CloseHandle(reinterpret_cast(this->_handle)); 82 | this->_handle = NULL; 83 | return 0; 84 | case WAIT_ABANDONED: 85 | return -2; 86 | case WAIT_TIMEOUT: 87 | return -1; 88 | } 89 | #else 90 | UNUSED(timeout); 91 | pthread_join((pthread_t)(this->_handle), NULL); 92 | #endif 93 | return 0; 94 | } 95 | 96 | bool operator== ( const Thread & right) { 97 | return this->_handle == right._handle; 98 | } 99 | protected: 100 | explicit Thread( thread_proc_t proc, void * param ):_param(param),_func(proc), _handle(0){} 101 | void * _param; 102 | thread_proc_t _func; 103 | _size_t _handle; 104 | }; 105 | 106 | -------------------------------------------------------------------------------- /rover_tool/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rover_tool 4 | 0.0.0 5 | The rover_tool package 6 | 7 | 8 | 9 | 10 | lingkang 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | joy 53 | roscpp 54 | joy 55 | roscpp 56 | joy 57 | roscpp 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /rover_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rover_driver 4 | 0.0.0 5 | The rover_driver package 6 | 7 | 8 | 9 | 10 | zlingkang 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | nav_msgs 53 | sensor_msgs 54 | tf 55 | nav_msgs 56 | sensor_msgs 57 | tf 58 | nav_msgs 59 | sensor_msgs 60 | tf 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /ydlidar/sdk/include/CYdLidar.h: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | #include "utils.h" 4 | #include "ydlidar_driver.h" 5 | #include 6 | 7 | 8 | #define PropertyBuilderByName(type, name, access_permission)\ 9 | access_permission:\ 10 | type m_##name;\ 11 | public:\ 12 | inline void set##name(type v) {\ 13 | m_##name = v;\ 14 | }\ 15 | inline type get##name() {\ 16 | return m_##name;\ 17 | }\ 18 | 19 | #define DEG2RAD(x) ((x)*M_PI/180.) 20 | 21 | class YDLIDAR_API CYdLidar 22 | { 23 | PropertyBuilderByName(float,MaxRange,private) 24 | PropertyBuilderByName(float,MinRange,private) 25 | PropertyBuilderByName(float,MaxAngle,private) 26 | PropertyBuilderByName(float,MinAngle,private) 27 | PropertyBuilderByName(int,ScanFrequency,private) 28 | 29 | PropertyBuilderByName(bool,Intensities,private) 30 | PropertyBuilderByName(bool,FixedResolution,private) 31 | PropertyBuilderByName(bool,Exposure,private) 32 | PropertyBuilderByName(bool,HeartBeat,private) 33 | PropertyBuilderByName(bool,Reversion, private) 34 | 35 | PropertyBuilderByName(int,SerialBaudrate,private) 36 | PropertyBuilderByName(int,SampleRate,private) 37 | 38 | PropertyBuilderByName(std::string,SerialPort,private) 39 | PropertyBuilderByName(std::vector,IgnoreArray,private) 40 | 41 | 42 | public: 43 | CYdLidar(); //!< Constructor 44 | virtual ~CYdLidar(); //!< Destructor: turns the laser off. 45 | 46 | bool initialize(); //!< Attempts to connect and turns the laser on. Raises an exception on error. 47 | 48 | // Return true if laser data acquistion succeeds, If it's not 49 | bool doProcessSimple(LaserScan &outscan, bool &hardwareError); 50 | 51 | //Turn on the motor enable 52 | bool turnOn(); //!< See base class docs 53 | //Turn off the motor enable and close the scan 54 | bool turnOff(); //!< See base class docs 55 | 56 | /** Returns true if the device is in good health, If it's not*/ 57 | bool getDeviceHealth() const; 58 | 59 | /** Returns true if the device information is correct, If it's not*/ 60 | bool getDeviceInfo(int &type); 61 | 62 | /** Retruns true if the heartbeat function is set to heart is successful, If it's not*/ 63 | bool checkHeartBeat() const; 64 | 65 | /** Retruns true if the scan frequency is set to user's frequency is successful, If it's not*/ 66 | bool checkScanFrequency(); 67 | 68 | //Turn off lidar connection 69 | void disconnecting(); //!< Closes the comms with the laser. Shouldn't have to be directly needed by the user 70 | 71 | protected: 72 | /** Returns true if communication has been established with the device. If it's not, 73 | * try to create a comms channel. 74 | * \return false on error. 75 | */ 76 | bool checkCOMMs(); 77 | 78 | /** Returns true if health status and device information has been obtained with the device. If it's not, 79 | * \return false on error. 80 | */ 81 | bool checkStatus(); 82 | 83 | /** Returns true if the normal scan runs with the device. If it's not, 84 | * \return false on error. 85 | */ 86 | bool checkHardware(); 87 | 88 | 89 | 90 | private: 91 | bool isScanning; 92 | int node_counts ; 93 | double each_angle; 94 | }; // End of class 95 | 96 | -------------------------------------------------------------------------------- /ydlidar/README.md: -------------------------------------------------------------------------------- 1 | YDLIDAR ROS PACKAGE V1.3.1 2 | ===================================================================== 3 | 4 | ROS node and test application for YDLIDAR 5 | 6 | Visit EAI Website for more details about YDLIDAR. 7 | 8 | How to build YDLIDAR ros package 9 | ===================================================================== 10 | 1) Clone this project to your catkin's workspace src folder 11 | 2) Running catkin_make to build ydlidar_node and ydlidar_client 12 | 3) Create the name "/dev/ydlidar" for YDLIDAR 13 | --$ roscd ydlidar/startup 14 | --$ sudo chmod 777 ./* 15 | --$ sudo sh initenv.sh 16 | 17 | How to run YDLIDAR ros package 18 | ===================================================================== 19 | There're two ways to run YDLIDAR ros package 20 | 21 | 1. Run YDLIDAR node and view in the rviz 22 | ------------------------------------------------------------ 23 | roslaunch ydlidar lidar_view.launch 24 | 25 | You should see YDLIDAR's scan result in the rviz. 26 | 27 | 2. Run YDLIDAR node and view using test application 28 | ------------------------------------------------------------ 29 | roslaunch ydlidar lidar.launch 30 | 31 | rosrun ydlidar ydlidar_client 32 | 33 | You should see YDLIDAR's scan result in the console 34 | 35 | 36 | Parameters 37 | ===================================================================== 38 | port (string, default: /dev/ydlidar) 39 | 40 | serial port name used in your system. 41 | 42 | baudrate (int, default: 115200) 43 | 44 | serial port baud rate. 45 | 46 | frame_id (string, default: laser_frame) 47 | 48 | frame ID for the device. 49 | 50 | low_exposure (low_exposure, default: false) 51 | 52 | indicated whether the LIDAR has low light power mode. 53 | 54 | angle_fixed (bool, default: true) 55 | 56 | indicated whether the driver needs do angle compensation. 57 | 58 | heartbeat (bool, default: false) 59 | 60 | indicated whether the LIDAR IS powered off. 61 | 62 | resolution_fixed (bool, default: true) 63 | 64 | indicated whether the LIDAR has a fixed angular resolution. 65 | 66 | angle_min (double, default: -180) 67 | 68 | Min valid angle (°) for LIDAR data. 69 | 70 | angle_max (double, default: 180) 71 | 72 | Max valid angle (°) for LIDAR data. 73 | 74 | range_min (double, default: 0.08) 75 | 76 | Min valid range (m) for LIDAR data. 77 | 78 | range_max (double, default: 16.0) 79 | 80 | Max valid range (m) for LIDAR data. 81 | 82 | ignore_array (string, default: "") 83 | 84 | Set the current angle range value to zero. 85 | 86 | samp_rate (int, default: 4) 87 | 88 | the LIDAR sampling frequency. 89 | 90 | frequency (double, default: 7) 91 | 92 | the LIDAR scanning frequency. 93 | 94 | 95 | 96 | 97 | Upgrade Log 98 | ===================================================================== 99 | 2018-04-16 version:1.3.1 100 | 101 | 1.Update SDK verison to 1.3.1 102 | 103 | 2.Increase sampling frequency,scan frequency setting. 104 | 105 | 3.Unified coordinate system. 106 | 107 | 4.Repair X4,S4 LIDAR cannot be opened. 108 | 109 | 5.Increased G4 G4C F4Pro LIDAR power-off protection. 110 | 111 | 6.Increased S4B LIDAR low optical power setting. 112 | 113 | 7.Fix the wait time for closing ros node. 114 | 115 | 8.Compensate for each laser point timestamp. 116 | 117 | 9.Unified profile, automatic correction lidar model. 118 | 119 | 120 | 121 | 122 | 123 | 124 | -------------------------------------------------------------------------------- /ydlidar/sdk/src/impl/unix/unix_serial.h: -------------------------------------------------------------------------------- 1 | #if !defined(_WIN32) 2 | 3 | #ifndef SERIAL_IMPL_UNIX_H 4 | #define SERIAL_IMPL_UNIX_H 5 | 6 | #include 7 | #include 8 | #include 9 | #include "serial.h" 10 | 11 | namespace serial { 12 | 13 | using std::size_t; 14 | using std::string; 15 | 16 | 17 | class MillisecondTimer { 18 | public: 19 | MillisecondTimer(const uint32_t millis); 20 | int64_t remaining(); 21 | 22 | private: 23 | static timespec timespec_now(); 24 | timespec expiry; 25 | }; 26 | 27 | class serial::Serial::SerialImpl { 28 | public: 29 | SerialImpl (const string &port, 30 | unsigned long baudrate, 31 | bytesize_t bytesize, 32 | parity_t parity, 33 | stopbits_t stopbits, 34 | flowcontrol_t flowcontrol); 35 | 36 | virtual ~SerialImpl (); 37 | 38 | bool open (); 39 | 40 | void close (); 41 | 42 | bool isOpen () const; 43 | 44 | size_t available (); 45 | 46 | bool waitReadable (uint32_t timeout); 47 | 48 | void waitByteTimes (size_t count); 49 | 50 | int waitfordata(size_t data_count, uint32_t timeout, size_t * returned_size); 51 | 52 | size_t read (uint8_t *buf, size_t size = 1); 53 | 54 | size_t write (const uint8_t *data, size_t length); 55 | 56 | 57 | void flush (); 58 | 59 | void flushInput (); 60 | 61 | void flushOutput (); 62 | 63 | void sendBreak (int duration); 64 | 65 | bool setBreak (bool level); 66 | 67 | bool setRTS (bool level); 68 | 69 | bool setDTR (bool level); 70 | 71 | bool waitForChange (); 72 | 73 | bool getCTS (); 74 | 75 | bool getDSR (); 76 | 77 | bool getRI (); 78 | 79 | bool getCD (); 80 | 81 | uint32_t getByteTime(); 82 | 83 | void setPort (const string &port); 84 | 85 | string getPort () const; 86 | 87 | void setTimeout (Timeout &timeout); 88 | 89 | Timeout getTimeout () const; 90 | 91 | bool setBaudrate (unsigned long baudrate); 92 | 93 | bool setStandardBaudRate(speed_t baudrate); 94 | 95 | bool setCustomBaudRate(unsigned long baudrate); 96 | 97 | unsigned long getBaudrate () const; 98 | 99 | bool setBytesize (bytesize_t bytesize); 100 | 101 | bytesize_t getBytesize () const; 102 | 103 | bool setParity (parity_t parity); 104 | 105 | parity_t getParity () const; 106 | 107 | bool setStopbits (stopbits_t stopbits); 108 | 109 | stopbits_t getStopbits () const; 110 | 111 | bool setFlowcontrol (flowcontrol_t flowcontrol); 112 | 113 | flowcontrol_t getFlowcontrol () const; 114 | 115 | bool setTermios(const termios *tio); 116 | 117 | bool getTermios(termios *tio); 118 | 119 | int readLock (); 120 | 121 | int readUnlock (); 122 | 123 | int writeLock (); 124 | 125 | int writeUnlock (); 126 | 127 | 128 | private: 129 | string port_; // Path to the file descriptor 130 | int fd_; // The current file descriptor 131 | 132 | bool is_open_; 133 | bool xonxoff_; 134 | bool rtscts_; 135 | 136 | Timeout timeout_; // Timeout for read operations 137 | unsigned long baudrate_; // Baudrate 138 | uint32_t byte_time_ns_; // Nanoseconds to transmit/receive a single byte 139 | 140 | parity_t parity_; // Parity 141 | bytesize_t bytesize_; // Size of the bytes 142 | stopbits_t stopbits_; // Stop Bits 143 | flowcontrol_t flowcontrol_; // Flow Control 144 | 145 | // Mutex used to lock the read functions 146 | pthread_mutex_t read_mutex; 147 | // Mutex used to lock the write functions 148 | pthread_mutex_t write_mutex; 149 | }; 150 | 151 | } 152 | 153 | #endif // SERIAL_IMPL_UNIX_H 154 | 155 | #endif // !defined(_WIN32) 156 | -------------------------------------------------------------------------------- /ydlidar/sdk/src/impl/windows/win_serial.h: -------------------------------------------------------------------------------- 1 | #if defined(_WIN32) 2 | 3 | #ifndef SERIAL_IMPL_WINDOWS_H 4 | #define SERIAL_IMPL_WINDOWS_H 5 | 6 | #include "serial.h" 7 | #include "time.h" 8 | #include "windows.h" 9 | 10 | namespace serial { 11 | 12 | using std::string; 13 | using std::wstring; 14 | using std::invalid_argument; 15 | 16 | 17 | class serial::Serial::SerialImpl { 18 | public: 19 | explicit SerialImpl (const string &port, 20 | unsigned long baudrate, 21 | bytesize_t bytesize, 22 | parity_t parity, 23 | stopbits_t stopbits, 24 | flowcontrol_t flowcontrol); 25 | 26 | virtual ~SerialImpl (); 27 | 28 | bool open (); 29 | 30 | void close (); 31 | 32 | bool isOpen () const; 33 | 34 | size_t available (); 35 | 36 | bool waitReadable (uint32_t timeout); 37 | 38 | void waitByteTimes (size_t count); 39 | 40 | int waitfordata(size_t data_count, uint32_t timeout, size_t * returned_size); 41 | 42 | size_t read (uint8_t *buf, size_t size = 1); 43 | 44 | size_t write (const uint8_t *data, size_t length); 45 | 46 | void flush (); 47 | 48 | void flushInput (); 49 | 50 | void flushOutput (); 51 | 52 | void sendBreak (int duration); 53 | 54 | bool setBreak (bool level); 55 | 56 | bool setRTS (bool level); 57 | 58 | bool setDTR (bool level); 59 | 60 | bool waitForChange (); 61 | 62 | bool getCTS (); 63 | 64 | bool getDSR (); 65 | 66 | bool getRI (); 67 | 68 | bool getCD (); 69 | 70 | uint32_t getByteTime(); 71 | 72 | void setPort (const string &port); 73 | 74 | string getPort () const; 75 | 76 | void setTimeout (Timeout &timeout); 77 | 78 | Timeout getTimeout () const; 79 | 80 | bool setBaudrate (unsigned long baudrate); 81 | 82 | unsigned long getBaudrate () const; 83 | 84 | bool setBytesize (bytesize_t bytesize); 85 | 86 | bytesize_t getBytesize () const; 87 | 88 | bool setParity (parity_t parity); 89 | 90 | parity_t getParity () const; 91 | 92 | bool setStopbits (stopbits_t stopbits); 93 | 94 | stopbits_t getStopbits () const; 95 | 96 | bool setFlowcontrol (flowcontrol_t flowcontrol); 97 | 98 | flowcontrol_t getFlowcontrol () const; 99 | 100 | 101 | bool setDcb(DCB *dcb); 102 | 103 | 104 | bool getDcb(DCB *dcb); 105 | 106 | int readLock (); 107 | 108 | int readUnlock (); 109 | 110 | int writeLock (); 111 | 112 | int writeUnlock (); 113 | 114 | protected: 115 | bool reconfigurePort (); 116 | 117 | public: 118 | enum { 119 | DEFAULT_RX_BUFFER_SIZE = 4096*4, 120 | DEFAULT_TX_BUFFER_SIZE = 128, 121 | }; 122 | 123 | 124 | private: 125 | wstring port_; // Path to the file descriptor 126 | HANDLE fd_; 127 | OVERLAPPED _wait_o; 128 | 129 | OVERLAPPED communicationOverlapped; 130 | OVERLAPPED readCompletionOverlapped; 131 | OVERLAPPED writeCompletionOverlapped; 132 | DWORD originalEventMask; 133 | DWORD triggeredEventMask; 134 | 135 | COMMTIMEOUTS currentCommTimeouts; 136 | COMMTIMEOUTS restoredCommTimeouts; 137 | 138 | bool is_open_; 139 | 140 | Timeout timeout_; // Timeout for read operations 141 | unsigned long baudrate_; // Baudrate 142 | uint32_t byte_time_ns_; // Nanoseconds to transmit/receive a single byte 143 | 144 | parity_t parity_; // Parity 145 | bytesize_t bytesize_; // Size of the bytes 146 | stopbits_t stopbits_; // Stop Bits 147 | flowcontrol_t flowcontrol_; // Flow Control 148 | 149 | // Mutex used to lock the read functions 150 | HANDLE read_mutex; 151 | // Mutex used to lock the write functions 152 | HANDLE write_mutex; 153 | }; 154 | 155 | } 156 | 157 | #endif // SERIAL_IMPL_WINDOWS_H 158 | 159 | #endif // if defined(_WIN32) 160 | -------------------------------------------------------------------------------- /ydlidar/sdk/src/impl/list_ports/list_ports_win.cpp: -------------------------------------------------------------------------------- 1 | #if defined(_WIN32) 2 | 3 | /* 4 | * Copyright (c) 2014 Craig Lilley 5 | * This software is made available under the terms of the MIT licence. 6 | * A copy of the licence can be obtained from: 7 | * http://opensource.org/licenses/MIT 8 | */ 9 | 10 | #include "serial.h" 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using serial::PortInfo; 19 | using std::vector; 20 | using std::string; 21 | 22 | static const DWORD port_name_max_length = 256; 23 | static const DWORD friendly_name_max_length = 256; 24 | static const DWORD hardware_id_max_length = 256; 25 | 26 | // Convert a wide Unicode string to an UTF8 string 27 | std::string utf8_encode(const std::wstring &wstr) 28 | { 29 | int size_needed = WideCharToMultiByte(CP_UTF8, 0, &wstr[0], (int)wstr.size(), NULL, 0, NULL, NULL); 30 | std::string strTo( size_needed, 0 ); 31 | WideCharToMultiByte (CP_UTF8, 0, &wstr[0], (int)wstr.size(), &strTo[0], size_needed, NULL, NULL); 32 | return strTo; 33 | } 34 | 35 | vector 36 | serial::list_ports() 37 | { 38 | vector devices_found; 39 | 40 | HDEVINFO device_info_set = SetupDiGetClassDevs( 41 | (const GUID *) &GUID_DEVCLASS_PORTS, 42 | NULL, 43 | NULL, 44 | DIGCF_PRESENT); 45 | 46 | unsigned int device_info_set_index = 0; 47 | SP_DEVINFO_DATA device_info_data; 48 | 49 | device_info_data.cbSize = sizeof(SP_DEVINFO_DATA); 50 | 51 | while(SetupDiEnumDeviceInfo(device_info_set, device_info_set_index, &device_info_data)) 52 | { 53 | device_info_set_index++; 54 | 55 | // Get port name 56 | 57 | HKEY hkey = SetupDiOpenDevRegKey( 58 | device_info_set, 59 | &device_info_data, 60 | DICS_FLAG_GLOBAL, 61 | 0, 62 | DIREG_DEV, 63 | KEY_READ); 64 | 65 | TCHAR port_name[port_name_max_length]; 66 | DWORD port_name_length = port_name_max_length; 67 | 68 | LONG return_code = RegQueryValueEx( 69 | hkey, 70 | _T("PortName"), 71 | NULL, 72 | NULL, 73 | (LPBYTE)port_name, 74 | &port_name_length); 75 | 76 | RegCloseKey(hkey); 77 | 78 | if(return_code != EXIT_SUCCESS) 79 | continue; 80 | 81 | if(port_name_length > 0 && port_name_length <= port_name_max_length) 82 | port_name[port_name_length-1] = '\0'; 83 | else 84 | port_name[0] = '\0'; 85 | 86 | // Ignore parallel ports 87 | 88 | if(_tcsstr(port_name, _T("LPT")) != NULL) 89 | continue; 90 | 91 | // Get port friendly name 92 | 93 | TCHAR friendly_name[friendly_name_max_length]; 94 | DWORD friendly_name_actual_length = 0; 95 | 96 | BOOL got_friendly_name = SetupDiGetDeviceRegistryProperty( 97 | device_info_set, 98 | &device_info_data, 99 | SPDRP_FRIENDLYNAME, 100 | NULL, 101 | (PBYTE)friendly_name, 102 | friendly_name_max_length, 103 | &friendly_name_actual_length); 104 | 105 | if(got_friendly_name == TRUE && friendly_name_actual_length > 0) 106 | friendly_name[friendly_name_actual_length-1] = '\0'; 107 | else 108 | friendly_name[0] = '\0'; 109 | 110 | // Get hardware ID 111 | 112 | TCHAR hardware_id[hardware_id_max_length]; 113 | DWORD hardware_id_actual_length = 0; 114 | 115 | BOOL got_hardware_id = SetupDiGetDeviceRegistryProperty( 116 | device_info_set, 117 | &device_info_data, 118 | SPDRP_HARDWAREID, 119 | NULL, 120 | (PBYTE)hardware_id, 121 | hardware_id_max_length, 122 | &hardware_id_actual_length); 123 | 124 | if(got_hardware_id == TRUE && hardware_id_actual_length > 0) 125 | hardware_id[hardware_id_actual_length-1] = '\0'; 126 | else 127 | hardware_id[0] = '\0'; 128 | 129 | #ifdef UNICODE 130 | std::string portName = utf8_encode(port_name); 131 | std::string friendlyName = utf8_encode(friendly_name); 132 | std::string hardwareId = utf8_encode(hardware_id); 133 | #else 134 | std::string portName = port_name; 135 | std::string friendlyName = friendly_name; 136 | std::string hardwareId = hardware_id; 137 | #endif 138 | 139 | PortInfo port_entry; 140 | port_entry.port = portName; 141 | port_entry.description = friendlyName; 142 | port_entry.hardware_id = hardwareId; 143 | 144 | devices_found.push_back(port_entry); 145 | } 146 | 147 | SetupDiDestroyDeviceInfoList(device_info_set); 148 | 149 | return devices_found; 150 | } 151 | 152 | #endif // #if defined(_WIN32) 153 | -------------------------------------------------------------------------------- /ydlidar/launch/lidar.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /LidarLaserScan1 10 | Splitter Ratio: 0.5 11 | Tree Height: 413 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.03 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Alpha: 1 52 | Autocompute Intensity Bounds: true 53 | Autocompute Value Bounds: 54 | Max Value: 0 55 | Min Value: 0 56 | Value: true 57 | Axis: Z 58 | Channel Name: intensity 59 | Class: rviz/LaserScan 60 | Color: 255; 255; 255 61 | Color Transformer: AxisColor 62 | Decay Time: 0 63 | Enabled: true 64 | Invert Rainbow: false 65 | Max Color: 255; 255; 255 66 | Max Intensity: 4096 67 | Min Color: 0; 0; 0 68 | Min Intensity: 0 69 | Name: LidarLaserScan 70 | Position Transformer: XYZ 71 | Queue Size: 1000 72 | Selectable: true 73 | Size (Pixels): 5 74 | Size (m): 0.03 75 | Style: Squares 76 | Topic: /scan 77 | Use Fixed Frame: true 78 | Use rainbow: true 79 | Value: true 80 | Enabled: true 81 | Global Options: 82 | Background Color: 48; 48; 48 83 | Fixed Frame: laser_frame 84 | Frame Rate: 30 85 | Name: root 86 | Tools: 87 | - Class: rviz/MoveCamera 88 | - Class: rviz/Interact 89 | Hide Inactive Objects: true 90 | - Class: rviz/Select 91 | - Class: rviz/SetInitialPose 92 | Topic: /initialpose 93 | - Class: rviz/SetGoal 94 | Topic: /move_base_simple/goal 95 | Value: true 96 | Views: 97 | Current: 98 | Class: rviz/Orbit 99 | Distance: 11.1184 100 | Focal Point: 101 | X: -0.0344972 102 | Y: 0.065886 103 | Z: 0.148092 104 | Name: Current View 105 | Near Clip Distance: 0.01 106 | Pitch: 0.615399 107 | Target Frame: 108 | Value: Orbit (rviz) 109 | Yaw: 5.82358 110 | Saved: ~ 111 | Window Geometry: 112 | Displays: 113 | collapsed: false 114 | Height: 626 115 | Hide Left Dock: false 116 | Hide Right Dock: false 117 | QMainWindow State: 000000ff00000000fd0000000400000000000001950000022cfc0200000006fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000022c000000dd00fffffffb0000000a0049006d006100670065000000028d000000c60000000000000000000000010000010b00000294fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003300000294000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003240000022c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 118 | Selection: 119 | collapsed: false 120 | Time: 121 | collapsed: false 122 | Tool Properties: 123 | collapsed: false 124 | Views: 125 | collapsed: false 126 | Width: 1215 127 | X: 65 128 | Y: 24 129 | -------------------------------------------------------------------------------- /rover_driver/src/rover_driver_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | const double CTRL_SCALE = 4.0; 8 | 9 | class RoverRobot 10 | { 11 | public: 12 | static RoverRobot *getInstance() 13 | { 14 | if(!s_instance_) 15 | s_instance_ = new RoverRobot; 16 | return s_instance_; 17 | } 18 | 19 | RoverRobot(const RoverRobot&)=delete; 20 | RoverRobot& operator=(const RoverRobot&)=delete; 21 | 22 | void motor1SpeedCB(const std_msgs::Float64::ConstPtr& msg); 23 | void motor2SpeedCB(const std_msgs::Float64::ConstPtr& msg); 24 | void velCmdCB(const geometry_msgs::Twist& msg); 25 | 26 | void odomPub(); 27 | private: 28 | RoverRobot(); 29 | static RoverRobot *s_instance_; 30 | 31 | ros::NodeHandle *node_; 32 | 33 | ros::Subscriber motor1_speed_sub_; 34 | ros::Subscriber motor2_speed_sub_; 35 | ros::Publisher odom_pub_; 36 | tf::TransformBroadcaster odom_broadcaster; 37 | float motor1_measured_speed_; 38 | float motor2_measured_speed_; 39 | ros::Time current_time_; 40 | ros::Time last_time_; 41 | double x_; 42 | double y_; 43 | double th_; 44 | double vx_; 45 | double vy_; 46 | double vth_; 47 | 48 | ros::Subscriber velocity_cmd_sub_; 49 | ros::Publisher motor1_speed_pub_; 50 | ros::Publisher motor2_speed_pub_; 51 | geometry_msgs::Twist vel_cmd_; 52 | 53 | float BASE_WIDTH_; 54 | 55 | }; 56 | 57 | void RoverRobot::motor1SpeedCB(const std_msgs::Float64::ConstPtr& msg) 58 | { 59 | motor1_measured_speed_ = msg->data/CTRL_SCALE; 60 | } 61 | void RoverRobot::motor2SpeedCB(const std_msgs::Float64::ConstPtr& msg) 62 | { 63 | motor2_measured_speed_ = msg->data/CTRL_SCALE; 64 | } 65 | 66 | void RoverRobot::velCmdCB(const geometry_msgs::Twist& msg) 67 | { 68 | geometry_msgs::Twist trgt_twist = msg; 69 | double v_linear = trgt_twist.linear.x * CTRL_SCALE; 70 | double vth = trgt_twist.angular.z * CTRL_SCALE; 71 | 72 | ROS_INFO_STREAM("[ROV] vel_cmd: " << v_linear << " " << vth); 73 | 74 | std_msgs::Float64 motor1_speed; 75 | std_msgs::Float64 motor2_speed; 76 | motor1_speed.data = vth*BASE_WIDTH_/2.0 + v_linear; 77 | motor2_speed.data = 2*v_linear - motor1_speed.data; 78 | motor1_speed_pub_.publish(motor1_speed); 79 | motor2_speed_pub_.publish(motor2_speed); 80 | } 81 | 82 | void RoverRobot::odomPub() 83 | { 84 | current_time_ = ros::Time::now(); 85 | double v_linear = (motor1_measured_speed_ + motor2_measured_speed_) / 2.0; 86 | double v_th = (motor1_measured_speed_ - motor2_measured_speed_) / BASE_WIDTH_; 87 | vx_ = v_linear; 88 | vy_ = 0; 89 | vth_ = v_th; 90 | 91 | double dt = (current_time_ - last_time_).toSec(); 92 | /* 93 | ROS_INFO_STREAM("vx_ : " << vx_); 94 | ROS_INFO_STREAM("vy_ : " << vy_); 95 | ROS_INFO_STREAM("vth_ : " << vth_); 96 | ROS_INFO_STREAM("dt : " << dt); 97 | */ 98 | double delta_th = vth_ * dt; 99 | double delta_x = (vx_ * cos(th_ + delta_th/2) - vy_*sin(th_ + delta_th/2)) * dt; 100 | double delta_y = (vx_ * sin(th_ + delta_th/2) - vy_*cos(th_ + delta_th/2)) * dt; 101 | 102 | x_ += delta_x; 103 | y_ += delta_y; 104 | th_ += delta_th; 105 | /* 106 | ROS_INFO_STREAM("x_ : " << x_); 107 | ROS_INFO_STREAM("y_ : " << y_); 108 | ROS_INFO_STREAM("th_ : " << th_); 109 | */ 110 | 111 | geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th_); 112 | 113 | geometry_msgs::TransformStamped odom_trans; 114 | odom_trans.header.stamp = current_time_; 115 | odom_trans.header.frame_id = "odom"; 116 | odom_trans.child_frame_id = "base_link"; 117 | odom_trans.transform.translation.x = x_; 118 | odom_trans.transform.translation.y = y_; 119 | odom_trans.transform.translation.z = 0.0; 120 | odom_trans.transform.rotation = odom_quat; 121 | odom_broadcaster.sendTransform(odom_trans); 122 | 123 | nav_msgs::Odometry odom; 124 | odom.header.stamp = current_time_; 125 | odom.header.frame_id = "odom"; 126 | odom.pose.pose.position.x = x_; 127 | odom.pose.pose.position.y = y_; 128 | odom.pose.pose.position.z = 0.0; 129 | odom.pose.pose.orientation = odom_quat; 130 | odom.child_frame_id = "base_link"; 131 | odom.twist.twist.linear.x = vx_; 132 | odom.twist.twist.linear.y = vy_; 133 | odom.twist.twist.angular.z = vth_; 134 | odom_pub_.publish(odom); 135 | 136 | last_time_ = current_time_; 137 | } 138 | 139 | template 140 | inline void get_param(const ros::NodeHandle* nh, const std::string& param_name, T& var, const T default_value) 141 | { 142 | nh->param(param_name, var, default_value); 143 | ROS_INFO_STREAM("[ROV] Param " << param_name << " : " << var); 144 | } 145 | 146 | RoverRobot::RoverRobot() 147 | { 148 | node_ = new ros::NodeHandle("~"); 149 | 150 | motor1_speed_sub_ = node_->subscribe("/motor1_measured_speed", 100, &RoverRobot::motor1SpeedCB, this); 151 | motor2_speed_sub_ = node_->subscribe("/motor2_measured_speed", 100, &RoverRobot::motor2SpeedCB, this); 152 | odom_pub_ = node_->advertise("/odom", 50); 153 | 154 | velocity_cmd_sub_ = node_->subscribe("/cmd_vel", 100, &RoverRobot::velCmdCB, this); 155 | motor1_speed_pub_ = node_->advertise("/motor1", 50); 156 | motor2_speed_pub_ = node_->advertise("/motor2", 50); 157 | 158 | get_param(node_, "base_width", BASE_WIDTH_, 0.16); 159 | 160 | x_ = 0.0; 161 | y_ = 0.0; 162 | th_ = 0.0; 163 | vx_ = 0.0; 164 | vy_ = 0.0; 165 | vth_ = 0.0; 166 | current_time_ = ros::Time::now(); 167 | last_time_ = ros::Time::now(); 168 | } 169 | 170 | RoverRobot *RoverRobot::s_instance_ = 0; 171 | 172 | int main(int argc, char** argv) 173 | { 174 | ros::init(argc, argv, "rover_driver_node"); 175 | 176 | RoverRobot* rover_ptr = RoverRobot::getInstance(); 177 | 178 | ros::Rate r(30); 179 | 180 | while(ros::ok()) 181 | { 182 | rover_ptr->odomPub(); 183 | ros::spinOnce(); 184 | } 185 | } 186 | -------------------------------------------------------------------------------- /ydlidar/sdk/include/locker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifdef _WIN32 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #else 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #endif 18 | 19 | 20 | class Locker 21 | { 22 | public: 23 | enum LOCK_STATUS { 24 | LOCK_OK = 0, 25 | LOCK_TIMEOUT = -1, 26 | LOCK_FAILED = -2 27 | }; 28 | 29 | Locker(){ 30 | #ifdef _WIN32 31 | _lock = NULL; 32 | #endif 33 | init(); 34 | } 35 | 36 | ~Locker(){ 37 | release(); 38 | } 39 | 40 | Locker::LOCK_STATUS lock(unsigned long timeout = 0xFFFFFFFF) { 41 | #ifdef _WIN32 42 | switch (WaitForSingleObject(_lock, timeout==0xFFFFFFF?INFINITE:(DWORD)timeout)) { 43 | case WAIT_ABANDONED: 44 | return LOCK_FAILED; 45 | case WAIT_OBJECT_0: 46 | return LOCK_OK; 47 | case WAIT_TIMEOUT: 48 | return LOCK_TIMEOUT; 49 | } 50 | 51 | #else 52 | #ifdef _MACOS 53 | if (timeout !=0 ) { 54 | if (pthread_mutex_lock(&_lock) == 0) return LOCK_OK; 55 | } 56 | #else 57 | if (timeout == 0xFFFFFFFF){ 58 | if (pthread_mutex_lock(&_lock) == 0) return LOCK_OK; 59 | } 60 | #endif 61 | else if (timeout == 0) { 62 | if (pthread_mutex_trylock(&_lock) == 0) return LOCK_OK; 63 | } 64 | #ifndef _MACOS 65 | else{ 66 | timespec wait_time; 67 | timeval now; 68 | gettimeofday(&now,NULL); 69 | 70 | wait_time.tv_sec = timeout/1000 + now.tv_sec; 71 | wait_time.tv_nsec = (timeout%1000)*1000000 + now.tv_usec*1000; 72 | 73 | if (wait_time.tv_nsec >= 1000000000){ 74 | ++wait_time.tv_sec; 75 | wait_time.tv_nsec -= 1000000000; 76 | } 77 | 78 | #if !defined(__ANDROID__) 79 | 80 | switch (pthread_mutex_timedlock(&_lock,&wait_time)){ 81 | case 0: 82 | return LOCK_OK; 83 | case ETIMEDOUT: 84 | return LOCK_TIMEOUT; 85 | } 86 | 87 | #else 88 | struct timeval timenow; 89 | struct timespec sleepytime; 90 | /* This is just to avoid a completely busy wait */ 91 | sleepytime.tv_sec = 0; 92 | sleepytime.tv_nsec = 10000000; /* 10ms */ 93 | 94 | while (pthread_mutex_trylock (&_lock) == EBUSY) { 95 | gettimeofday (&timenow, NULL); 96 | 97 | if (timenow.tv_sec >= wait_time.tv_sec && 98 | (timenow.tv_usec * 1000) >= wait_time.tv_nsec) { 99 | return LOCK_TIMEOUT; 100 | } 101 | 102 | nanosleep (&sleepytime, NULL); 103 | } 104 | 105 | return LOCK_OK; 106 | 107 | #endif 108 | 109 | } 110 | #endif 111 | #endif 112 | 113 | return LOCK_FAILED; 114 | } 115 | 116 | 117 | void unlock(){ 118 | #ifdef _WIN32 119 | ReleaseMutex(_lock); 120 | #else 121 | pthread_mutex_unlock(&_lock); 122 | #endif 123 | } 124 | 125 | #ifdef _WIN32 126 | HANDLE getLockHandle(){ 127 | return _lock; 128 | } 129 | #else 130 | pthread_mutex_t *getLockHandle(){ 131 | return &_lock; 132 | } 133 | #endif 134 | 135 | 136 | 137 | protected: 138 | void init(){ 139 | #ifdef _WIN32 140 | _lock = CreateMutex(NULL,FALSE,NULL); 141 | #else 142 | pthread_mutex_init(&_lock, NULL); 143 | #endif 144 | } 145 | 146 | void release(){ 147 | unlock(); 148 | #ifdef _WIN32 149 | 150 | if(_lock){ 151 | CloseHandle(_lock); 152 | } 153 | _lock = NULL; 154 | #else 155 | pthread_mutex_destroy(&_lock); 156 | #endif 157 | } 158 | 159 | #ifdef _WIN32 160 | HANDLE _lock; 161 | #else 162 | pthread_mutex_t _lock; 163 | #endif 164 | }; 165 | 166 | 167 | class Event 168 | { 169 | public: 170 | 171 | enum { 172 | EVENT_OK = 1, 173 | EVENT_TIMEOUT = 2, 174 | EVENT_FAILED = 0, 175 | }; 176 | 177 | Event(bool isAutoReset = true, bool isSignal = false) 178 | #ifdef _WIN32 179 | : _event(NULL) 180 | #else 181 | : _is_signalled(isSignal) 182 | , _isAutoReset(isAutoReset) 183 | #endif 184 | { 185 | #ifdef _WIN32 186 | _event = CreateEvent(NULL, isAutoReset?FALSE:TRUE, isSignal?TRUE:FALSE, NULL); 187 | #else 188 | pthread_mutex_init(&_cond_locker, NULL); 189 | pthread_cond_init(&_cond_var, NULL); 190 | #endif 191 | } 192 | 193 | ~ Event(){ 194 | release(); 195 | } 196 | 197 | void set( bool isSignal = true ){ 198 | if (isSignal){ 199 | #ifdef _WIN32 200 | SetEvent(_event); 201 | #else 202 | pthread_mutex_lock(&_cond_locker); 203 | 204 | if ( _is_signalled == false ){ 205 | _is_signalled = true; 206 | pthread_cond_signal(&_cond_var); 207 | } 208 | pthread_mutex_unlock(&_cond_locker); 209 | #endif 210 | }else{ 211 | #ifdef _WIN32 212 | ResetEvent(_event); 213 | #else 214 | pthread_mutex_lock(&_cond_locker); 215 | _is_signalled = false; 216 | pthread_mutex_unlock(&_cond_locker); 217 | #endif 218 | } 219 | } 220 | 221 | unsigned long wait( unsigned long timeout = 0xFFFFFFFF ){ 222 | #ifdef _WIN32 223 | switch (WaitForSingleObject(_event, timeout==0xFFFFFFF?INFINITE:(DWORD)timeout)){ 224 | case WAIT_FAILED: 225 | return EVENT_FAILED; 226 | case WAIT_OBJECT_0: 227 | return EVENT_OK; 228 | case WAIT_TIMEOUT: 229 | return EVENT_TIMEOUT; 230 | } 231 | return EVENT_OK; 232 | #else 233 | unsigned long ans = EVENT_OK; 234 | pthread_mutex_lock( &_cond_locker ); 235 | 236 | if (!_is_signalled){ 237 | if (timeout == 0xFFFFFFFF){ 238 | pthread_cond_wait(&_cond_var,&_cond_locker); 239 | }else{ 240 | timespec wait_time; 241 | timeval now; 242 | gettimeofday(&now,NULL); 243 | 244 | wait_time.tv_sec = timeout/1000 + now.tv_sec; 245 | wait_time.tv_nsec = (timeout%1000)*1000000ULL + now.tv_usec*1000; 246 | 247 | if (wait_time.tv_nsec >= 1000000000){ 248 | ++wait_time.tv_sec; 249 | wait_time.tv_nsec -= 1000000000; 250 | } 251 | switch (pthread_cond_timedwait(&_cond_var,&_cond_locker,&wait_time)){ 252 | case 0: 253 | // signalled 254 | break; 255 | case ETIMEDOUT: 256 | // time up 257 | ans = EVENT_TIMEOUT; 258 | goto _final; 259 | break; 260 | default: 261 | ans = EVENT_FAILED; 262 | goto _final; 263 | } 264 | 265 | } 266 | } 267 | 268 | assert(_is_signalled); 269 | 270 | if (_isAutoReset){ 271 | _is_signalled = false; 272 | } 273 | _final: 274 | pthread_mutex_unlock( &_cond_locker ); 275 | 276 | return ans; 277 | #endif 278 | 279 | } 280 | protected: 281 | 282 | void release() { 283 | #ifdef _WIN32 284 | CloseHandle(_event); 285 | #else 286 | pthread_mutex_destroy(&_cond_locker); 287 | pthread_cond_destroy(&_cond_var); 288 | #endif 289 | } 290 | 291 | #ifdef _WIN32 292 | HANDLE _event; 293 | #else 294 | pthread_cond_t _cond_var; 295 | pthread_mutex_t _cond_locker; 296 | bool _is_signalled; 297 | bool _isAutoReset; 298 | #endif 299 | }; 300 | 301 | class ScopedLocker 302 | { 303 | public : 304 | ScopedLocker(Locker &l): _binded(l) { 305 | _binded.lock(); 306 | } 307 | 308 | void forceUnlock() { 309 | _binded.unlock(); 310 | } 311 | ~ScopedLocker() { 312 | _binded.unlock(); 313 | } 314 | Locker & _binded; 315 | }; 316 | 317 | 318 | -------------------------------------------------------------------------------- /rover_2dnav/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rover_2dnav) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | move_base 12 | ) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 21 | # catkin_python_setup() 22 | 23 | ################################################ 24 | ## Declare ROS messages, services and actions ## 25 | ################################################ 26 | 27 | ## To declare and build messages, services or actions from within this 28 | ## package, follow these steps: 29 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 30 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 31 | ## * In the file package.xml: 32 | ## * add a build_depend tag for "message_generation" 33 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 34 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 35 | ## but can be declared for certainty nonetheless: 36 | ## * add a run_depend tag for "message_runtime" 37 | ## * In this file (CMakeLists.txt): 38 | ## * add "message_generation" and every package in MSG_DEP_SET to 39 | ## find_package(catkin REQUIRED COMPONENTS ...) 40 | ## * add "message_runtime" and every package in MSG_DEP_SET to 41 | ## catkin_package(CATKIN_DEPENDS ...) 42 | ## * uncomment the add_*_files sections below as needed 43 | ## and list every .msg/.srv/.action file to be processed 44 | ## * uncomment the generate_messages entry below 45 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 46 | 47 | ## Generate messages in the 'msg' folder 48 | # add_message_files( 49 | # FILES 50 | # Message1.msg 51 | # Message2.msg 52 | # ) 53 | 54 | ## Generate services in the 'srv' folder 55 | # add_service_files( 56 | # FILES 57 | # Service1.srv 58 | # Service2.srv 59 | # ) 60 | 61 | ## Generate actions in the 'action' folder 62 | # add_action_files( 63 | # FILES 64 | # Action1.action 65 | # Action2.action 66 | # ) 67 | 68 | ## Generate added messages and services with any dependencies listed here 69 | # generate_messages( 70 | # DEPENDENCIES 71 | # std_msgs # Or other packages containing msgs 72 | # ) 73 | 74 | ################################################ 75 | ## Declare ROS dynamic reconfigure parameters ## 76 | ################################################ 77 | 78 | ## To declare and build dynamic reconfigure parameters within this 79 | ## package, follow these steps: 80 | ## * In the file package.xml: 81 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 82 | ## * In this file (CMakeLists.txt): 83 | ## * add "dynamic_reconfigure" to 84 | ## find_package(catkin REQUIRED COMPONENTS ...) 85 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 86 | ## and list every .cfg file to be processed 87 | 88 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 89 | # generate_dynamic_reconfigure_options( 90 | # cfg/DynReconf1.cfg 91 | # cfg/DynReconf2.cfg 92 | # ) 93 | 94 | ################################### 95 | ## catkin specific configuration ## 96 | ################################### 97 | ## The catkin_package macro generates cmake config files for your package 98 | ## Declare things to be passed to dependent projects 99 | ## INCLUDE_DIRS: uncomment this if your package contains header files 100 | ## LIBRARIES: libraries you create in this project that dependent projects also need 101 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 102 | ## DEPENDS: system dependencies of this project that dependent projects also need 103 | catkin_package( 104 | # INCLUDE_DIRS include 105 | # LIBRARIES rover_2dnav 106 | # CATKIN_DEPENDS move_base 107 | # DEPENDS system_lib 108 | ) 109 | 110 | ########### 111 | ## Build ## 112 | ########### 113 | 114 | ## Specify additional locations of header files 115 | ## Your package locations should be listed before other locations 116 | include_directories( 117 | # include 118 | ${catkin_INCLUDE_DIRS} 119 | ) 120 | 121 | ## Declare a C++ library 122 | # add_library(${PROJECT_NAME} 123 | # src/${PROJECT_NAME}/rover_2dnav.cpp 124 | # ) 125 | 126 | ## Add cmake target dependencies of the library 127 | ## as an example, code may need to be generated before libraries 128 | ## either from message generation or dynamic reconfigure 129 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 130 | 131 | ## Declare a C++ executable 132 | ## With catkin_make all packages are built within a single CMake context 133 | ## The recommended prefix ensures that target names across packages don't collide 134 | # add_executable(${PROJECT_NAME}_node src/rover_2dnav_node.cpp) 135 | 136 | ## Rename C++ executable without prefix 137 | ## The above recommended prefix causes long target names, the following renames the 138 | ## target back to the shorter version for ease of user use 139 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 140 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 141 | 142 | ## Add cmake target dependencies of the executable 143 | ## same as for the library above 144 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 145 | 146 | ## Specify libraries to link a library or executable target against 147 | # target_link_libraries(${PROJECT_NAME}_node 148 | # ${catkin_LIBRARIES} 149 | # ) 150 | 151 | ############# 152 | ## Install ## 153 | ############# 154 | 155 | # all install targets should use catkin DESTINATION variables 156 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 157 | 158 | ## Mark executable scripts (Python etc.) for installation 159 | ## in contrast to setup.py, you can choose the destination 160 | # install(PROGRAMS 161 | # scripts/my_python_script 162 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 163 | # ) 164 | 165 | ## Mark executables and/or libraries for installation 166 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 167 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 168 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 169 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 170 | # ) 171 | 172 | ## Mark cpp header files for installation 173 | # install(DIRECTORY include/${PROJECT_NAME}/ 174 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 175 | # FILES_MATCHING PATTERN "*.h" 176 | # PATTERN ".svn" EXCLUDE 177 | # ) 178 | 179 | ## Mark other files for installation (e.g. launch and bag files, etc.) 180 | # install(FILES 181 | # # myfile1 182 | # # myfile2 183 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 184 | # ) 185 | 186 | ############# 187 | ## Testing ## 188 | ############# 189 | 190 | ## Add gtest based cpp test target and link libraries 191 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_rover_2dnav.cpp) 192 | # if(TARGET ${PROJECT_NAME}-test) 193 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 194 | # endif() 195 | 196 | ## Add folders to be run by python nosetests 197 | # catkin_add_nosetests(test) 198 | -------------------------------------------------------------------------------- /rover_driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rover_driver) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | nav_msgs 12 | sensor_msgs 13 | tf 14 | ) 15 | 16 | add_definitions(-std=c++11) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | # find_package(Boost REQUIRED COMPONENTS system) 20 | 21 | 22 | ## Uncomment this if the package has a setup.py. This macro ensures 23 | ## modules and global scripts declared therein get installed 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 25 | # catkin_python_setup() 26 | 27 | ################################################ 28 | ## Declare ROS messages, services and actions ## 29 | ################################################ 30 | 31 | ## To declare and build messages, services or actions from within this 32 | ## package, follow these steps: 33 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 34 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 35 | ## * In the file package.xml: 36 | ## * add a build_depend tag for "message_generation" 37 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 38 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 39 | ## but can be declared for certainty nonetheless: 40 | ## * add a run_depend tag for "message_runtime" 41 | ## * In this file (CMakeLists.txt): 42 | ## * add "message_generation" and every package in MSG_DEP_SET to 43 | ## find_package(catkin REQUIRED COMPONENTS ...) 44 | ## * add "message_runtime" and every package in MSG_DEP_SET to 45 | ## catkin_package(CATKIN_DEPENDS ...) 46 | ## * uncomment the add_*_files sections below as needed 47 | ## and list every .msg/.srv/.action file to be processed 48 | ## * uncomment the generate_messages entry below 49 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 50 | 51 | ## Generate messages in the 'msg' folder 52 | # add_message_files( 53 | # FILES 54 | # Message1.msg 55 | # Message2.msg 56 | # ) 57 | 58 | ## Generate services in the 'srv' folder 59 | # add_service_files( 60 | # FILES 61 | # Service1.srv 62 | # Service2.srv 63 | # ) 64 | 65 | ## Generate actions in the 'action' folder 66 | # add_action_files( 67 | # FILES 68 | # Action1.action 69 | # Action2.action 70 | # ) 71 | 72 | ## Generate added messages and services with any dependencies listed here 73 | # generate_messages( 74 | # DEPENDENCIES 75 | # nav_msgs 76 | # ) 77 | 78 | ################################################ 79 | ## Declare ROS dynamic reconfigure parameters ## 80 | ################################################ 81 | 82 | ## To declare and build dynamic reconfigure parameters within this 83 | ## package, follow these steps: 84 | ## * In the file package.xml: 85 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 86 | ## * In this file (CMakeLists.txt): 87 | ## * add "dynamic_reconfigure" to 88 | ## find_package(catkin REQUIRED COMPONENTS ...) 89 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 90 | ## and list every .cfg file to be processed 91 | 92 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 93 | # generate_dynamic_reconfigure_options( 94 | # cfg/DynReconf1.cfg 95 | # cfg/DynReconf2.cfg 96 | # ) 97 | 98 | ################################### 99 | ## catkin specific configuration ## 100 | ################################### 101 | ## The catkin_package macro generates cmake config files for your package 102 | ## Declare things to be passed to dependent projects 103 | ## INCLUDE_DIRS: uncomment this if your package contains header files 104 | ## LIBRARIES: libraries you create in this project that dependent projects also need 105 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 106 | ## DEPENDS: system dependencies of this project that dependent projects also need 107 | catkin_package( 108 | # INCLUDE_DIRS include 109 | # LIBRARIES rover_driver 110 | # CATKIN_DEPENDS nav_msgs tf 111 | # DEPENDS system_lib 112 | ) 113 | 114 | ########### 115 | ## Build ## 116 | ########### 117 | 118 | ## Specify additional locations of header files 119 | ## Your package locations should be listed before other locations 120 | include_directories( 121 | # include 122 | ${catkin_INCLUDE_DIRS} 123 | ) 124 | 125 | ## Declare a C++ library 126 | # add_library(${PROJECT_NAME} 127 | # src/${PROJECT_NAME}/rover_driver.cpp 128 | # ) 129 | 130 | ## Add cmake target dependencies of the library 131 | ## as an example, code may need to be generated before libraries 132 | ## either from message generation or dynamic reconfigure 133 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 134 | 135 | ## Declare a C++ executable 136 | ## With catkin_make all packages are built within a single CMake context 137 | ## The recommended prefix ensures that target names across packages don't collide 138 | add_executable(${PROJECT_NAME}_node src/rover_driver_node.cpp) 139 | ## Rename C++ executable without prefix 140 | ## The above recommended prefix causes long target names, the following renames the 141 | ## target back to the shorter version for ease of user use 142 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 143 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 144 | 145 | ## Add cmake target dependencies of the executable 146 | ## same as for the library above 147 | add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 148 | 149 | ## Specify libraries to link a library or executable target against 150 | target_link_libraries(${PROJECT_NAME}_node 151 | ${catkin_LIBRARIES} 152 | ) 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | # install(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables and/or libraries for installation 168 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 169 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 170 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark cpp header files for installation 175 | # install(DIRECTORY include/${PROJECT_NAME}/ 176 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 177 | # FILES_MATCHING PATTERN "*.h" 178 | # PATTERN ".svn" EXCLUDE 179 | # ) 180 | 181 | ## Mark other files for installation (e.g. launch and bag files, etc.) 182 | # install(FILES 183 | # # myfile1 184 | # # myfile2 185 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 186 | # ) 187 | 188 | ############# 189 | ## Testing ## 190 | ############# 191 | 192 | ## Add gtest based cpp test target and link libraries 193 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_rover_driver.cpp) 194 | # if(TARGET ${PROJECT_NAME}-test) 195 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 196 | # endif() 197 | 198 | ## Add folders to be run by python nosetests 199 | # catkin_add_nosetests(test) 200 | -------------------------------------------------------------------------------- /rover_tool/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rover_tool) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | sensor_msgs 12 | roscpp 13 | ) 14 | 15 | add_definitions(-std=c++11) 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a run_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs # Or other packages containing msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES rover_tool 108 | # CATKIN_DEPENDS joy roscpp 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/rover_tool.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | # add_executable(${PROJECT_NAME}_node src/rover_tool_node.cpp) 137 | add_executable(tele_op_node src/tele_op_node.cpp) 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | target_link_libraries(tele_op_node 144 | ${catkin_LIBRARIES} 145 | ) 146 | 147 | ## Add cmake target dependencies of the executable 148 | ## same as for the library above 149 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 150 | 151 | ## Specify libraries to link a library or executable target against 152 | # target_link_libraries(${PROJECT_NAME}_node 153 | # ${catkin_LIBRARIES} 154 | # ) 155 | 156 | ############# 157 | ## Install ## 158 | ############# 159 | 160 | # all install targets should use catkin DESTINATION variables 161 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 162 | 163 | ## Mark executable scripts (Python etc.) for installation 164 | ## in contrast to setup.py, you can choose the destination 165 | # install(PROGRAMS 166 | # scripts/my_python_script 167 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | # ) 169 | 170 | ## Mark executables and/or libraries for installation 171 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_rover_tool.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /STM32-firmware/main.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include "hardwareserial.h" 4 | #include "gy85.h" 5 | #include "motor.h" 6 | #include "encoder.h" 7 | #include "led.h" 8 | #include "battery.h" 9 | #include "servo.h" 10 | //#include "intencoder.h" 11 | #include "sonar.h" 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #define DEBUG_RATE 120 24 | 25 | const float BASE_WIDTH = 0.16; // meter 26 | int Motor::counts_per_rev = 390; 27 | float Motor::wheel_diameter = 0.06; //meter 28 | int Motor::max_rpm = 1.5 * 60 / (3.14 * 0.06); //maximum speed 1.5m/s 29 | float Motor::Kp = 0.4; 30 | float Motor::Ki = 0; 31 | float Motor::Kd = 0; 32 | 33 | Led led; 34 | void led_cb(const std_msgs::Float64& cmd_msg){ 35 | static bool stat_led = true; 36 | if(stat_led){ 37 | led.on_off(ON); 38 | stat_led = false; 39 | }else{ 40 | led.on_off(OFF); 41 | stat_led = true; 42 | } 43 | } 44 | 45 | int motor1_pwm = 0; 46 | int motor2_pwm = 0; 47 | float motor1_speed = 0; 48 | float motor2_speed = 0; 49 | 50 | void motor1_cb(const std_msgs::Float64& cmd_msg) 51 | { 52 | motor1_speed = cmd_msg.data; //meters per second 53 | } 54 | void motor2_cb(const std_msgs::Float64& cmd_msg) 55 | { 56 | motor2_speed = cmd_msg.data; //meters per second 57 | } 58 | 59 | void kp_cb(const std_msgs::Float64& cmd_msg) 60 | { 61 | Motor::Kp = cmd_msg.data; 62 | } 63 | void ki_cb(const std_msgs::Float64& cmd_msg) 64 | { 65 | Motor::Ki = cmd_msg.data; 66 | } 67 | void kd_cb(const std_msgs::Float64& cmd_msg) 68 | { 69 | Motor::Kd = cmd_msg.data; 70 | } 71 | 72 | int main(void) 73 | { 74 | SystemInit(); 75 | initialise(); 76 | 77 | float length_per_rev = 3.14 * Motor::wheel_diameter; 78 | uint32_t previous_debug_time = 0; 79 | uint32_t previous_trans_time = 0; 80 | char buffer[50]; 81 | std_msgs::String str_msg; 82 | std_msgs::Float64 motor1_speed_msg; 83 | std_msgs::Float64 motor2_speed_msg; 84 | ros::NodeHandle nh; 85 | nh.initNode(); 86 | ros::Publisher battery("battery", &str_msg); 87 | ros::Publisher motor1_pub("motor1_measured_speed", &motor1_speed_msg); 88 | ros::Publisher motor2_pub("motor2_measured_speed", &motor2_speed_msg); 89 | /* 90 | tf::TransformBroadcaster odom_broadcaster; 91 | nav_msgs::Odometry odom; 92 | ros::Publisher odom_pub("odom", &odom); 93 | */ 94 | ros::Subscriber sub("led", led_cb); 95 | ros::Subscriber motor1_sub("motor1", motor1_cb); 96 | ros::Subscriber motor2_sub("motor2", motor2_cb); 97 | ros::Subscriber kp_sub("kp", kp_cb); 98 | ros::Subscriber ki_sub("ki", ki_cb); 99 | ros::Subscriber kd_sub("kd", kd_cb); 100 | 101 | nh.advertise(battery); 102 | //nh.advertise(odom_pub); 103 | nh.advertise(motor1_pub); 104 | nh.advertise(motor2_pub); 105 | nh.subscribe(sub); 106 | nh.subscribe(kp_sub); 107 | nh.subscribe(ki_sub); 108 | nh.subscribe(kd_sub); 109 | nh.subscribe(motor1_sub); 110 | nh.subscribe(motor2_sub); 111 | 112 | Battery bat(25, 10.6, 12.6); 113 | bat.init(); 114 | led.init(); 115 | //Serial.print("init motor \r\n"); 116 | Motor motor1(MOTOR1, 254, 575); 117 | Motor motor2(MOTOR2, 254, 575); 118 | motor1.init(); 119 | motor2.init(); 120 | motor1.spin(0); 121 | motor2.spin(0); 122 | Encoder encoder1(ENCODER1, 0xffff, 0); 123 | Encoder encoder2(ENCODER2, 0xffff, 0); 124 | encoder1.init(); 125 | encoder2.init(); 126 | 127 | Vector gyros, acc, mag; 128 | Gy85 imu; 129 | imu.init(); 130 | 131 | double x = 0.0; 132 | double y = 0.0; 133 | double th = 0.0; 134 | 135 | double vx = 0.0; 136 | double vy = 0.0; 137 | double vth = 0.0; 138 | 139 | //ros::Time current_time; 140 | int trans_count = 0; 141 | while(1){ 142 | if ((millis() - previous_debug_time) >= (1000 / DEBUG_RATE)) { 143 | //Serial.print("encoder1 count : %ld\r\n", encoder1.read()); 144 | //Serial.print("encoder2 count : %ld\r\n", encoder2.read()); 145 | sprintf (buffer, "Current borad volt is : %f", bat.get_volt()); 146 | str_msg.data = buffer; 147 | //battery.publish( &str_msg ); 148 | 149 | // read and publish encoder reading 150 | motor1.calculate_rpm(encoder2.read()); 151 | motor2.calculate_rpm(encoder1.read()); 152 | // calculate required pwm 153 | motor1.required_rpm = motor1_speed * 60 / length_per_rev; 154 | motor2.required_rpm = motor2_speed * 60 / length_per_rev; 155 | motor1_pwm = motor1.calculate_pwm(); 156 | motor2_pwm = motor2.calculate_pwm(); 157 | motor1.spin(-motor1_pwm);// (-(int)motor1_speed); //(-motor1_pwm); 158 | motor2.spin(-motor2_pwm);// (-(int)motor1_speed); //(-motor1_pwm); 159 | //pwm1_msg.data = motor1_pwm; 160 | //pwm1_pub.publish( &pwm1_msg ); 161 | 162 | 163 | if(imu.check_gyroscope()){ 164 | gyros = imu.measure_gyroscope(); 165 | //Serial.print("gyros x: %f, y : %f, z: %f \r\n", gyros.x, gyros.y, gyros.z); 166 | } 167 | if(imu.check_accelerometer()){ 168 | acc = imu.measure_acceleration(); 169 | //Serial.print("acceleration x: %f, y : %f, z: %f \r\n", acc.x, acc.y, acc.z); 170 | } 171 | if(imu.check_magnetometer()){ 172 | mag = imu.measure_magnetometer(); 173 | //Serial.print("magnetometer x: %f, y : %f, z: %f \r\n", mag.x, mag.y, mag.z); 174 | } 175 | 176 | trans_count ++; 177 | if(trans_count >= 0){ 178 | 179 | trans_count = 0; 180 | motor1_speed_msg.data = motor1.current_rpm * length_per_rev/60.0; 181 | motor2_speed_msg.data = motor2.current_rpm * length_per_rev/60.0; 182 | motor1_pub.publish(&motor1_speed_msg); 183 | motor2_pub.publish(&motor2_speed_msg); 184 | /* 185 | double v_linear = (motor1.current_rpm + motor2.current_rpm) * length_per_rev/60.0; 186 | double v_th = (motor1.current_rpm - motor2.current_rpm) * length_per_rev/60.0 / BASE_WIDTH; 187 | vx = v_linear; 188 | vy = 0; 189 | vth = v_th; 190 | 191 | double dt = (double)(millis() - previous_trans_time) / 1000.0; 192 | 193 | double delta_th = vth * dt; 194 | double delta_x = (vx * cos(th + delta_th/2) - vy*sin(th + delta_th/2)) * dt; 195 | double delta_y = (vx * sin(th + delta_th/2) - vy*cos(th + delta_th/2)) * dt; 196 | 197 | x += delta_x; 198 | y += delta_y; 199 | th += delta_th; 200 | 201 | //current_time = ros::Time::now(); 202 | 203 | geometry_msgs::Quaternion odom_quat = tf::createQuaternionFromYaw(th); 204 | 205 | geometry_msgs::TransformStamped odom_trans; 206 | odom_trans.header.stamp = nh.now();//current_time; 207 | odom_trans.header.frame_id = "odom"; 208 | odom_trans.child_frame_id = "base_link"; 209 | odom_trans.transform.translation.x = x; 210 | odom_trans.transform.translation.y = y; 211 | odom_trans.transform.translation.z = 0.0; 212 | odom_trans.transform.rotation = odom_quat; 213 | odom_broadcaster.sendTransform(odom_trans); 214 | 215 | odom.header.stamp = nh.now(); 216 | odom.header.frame_id = "odom"; 217 | odom.pose.pose.position.x = x; 218 | odom.pose.pose.position.y = y; 219 | odom.pose.pose.position.z = 0.0; 220 | odom.pose.pose.orientation = odom_quat; 221 | odom.child_frame_id = "base_link"; 222 | odom.twist.twist.linear.x = vx; 223 | odom.twist.twist.linear.y = vy; 224 | odom.twist.twist.angular.z = vth; 225 | odom_pub.publish(&odom); 226 | */ 227 | previous_trans_time = millis(); 228 | 229 | } 230 | 231 | previous_debug_time = millis(); 232 | } 233 | nh.spinOnce(); 234 | } 235 | 236 | 237 | } 238 | 239 | 240 | -------------------------------------------------------------------------------- /ydlidar/sdk/src/impl/list_ports/list_ports_osx.cpp: -------------------------------------------------------------------------------- 1 | #if defined(__APPLE__) 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include "serial/serial.h" 16 | 17 | using serial::PortInfo; 18 | using std::string; 19 | using std::vector; 20 | 21 | #define HARDWARE_ID_STRING_LENGTH 128 22 | 23 | string cfstring_to_string( CFStringRef cfstring ); 24 | string get_device_path( io_object_t& serial_port ); 25 | string get_class_name( io_object_t& obj ); 26 | io_registry_entry_t get_parent_iousb_device( io_object_t& serial_port ); 27 | string get_string_property( io_object_t& device, const char* property ); 28 | uint16_t get_int_property( io_object_t& device, const char* property ); 29 | string rtrim(const string& str); 30 | 31 | string 32 | cfstring_to_string( CFStringRef cfstring ) 33 | { 34 | char cstring[MAXPATHLEN]; 35 | string result; 36 | 37 | if( cfstring ) 38 | { 39 | Boolean success = CFStringGetCString( cfstring, 40 | cstring, 41 | sizeof(cstring), 42 | kCFStringEncodingASCII ); 43 | 44 | if( success ) 45 | result = cstring; 46 | } 47 | 48 | return result; 49 | } 50 | 51 | string 52 | get_device_path( io_object_t& serial_port ) 53 | { 54 | CFTypeRef callout_path; 55 | string device_path; 56 | 57 | callout_path = IORegistryEntryCreateCFProperty( serial_port, 58 | CFSTR(kIOCalloutDeviceKey), 59 | kCFAllocatorDefault, 60 | 0 ); 61 | 62 | if (callout_path) 63 | { 64 | if( CFGetTypeID(callout_path) == CFStringGetTypeID() ) 65 | device_path = cfstring_to_string( static_cast(callout_path) ); 66 | 67 | CFRelease(callout_path); 68 | } 69 | 70 | return device_path; 71 | } 72 | 73 | string 74 | get_class_name( io_object_t& obj ) 75 | { 76 | string result; 77 | io_name_t class_name; 78 | kern_return_t kern_result; 79 | 80 | kern_result = IOObjectGetClass( obj, class_name ); 81 | 82 | if( kern_result == KERN_SUCCESS ) 83 | result = class_name; 84 | 85 | return result; 86 | } 87 | 88 | io_registry_entry_t 89 | get_parent_iousb_device( io_object_t& serial_port ) 90 | { 91 | io_object_t device = serial_port; 92 | io_registry_entry_t parent = 0; 93 | io_registry_entry_t result = 0; 94 | kern_return_t kern_result = KERN_FAILURE; 95 | string name = get_class_name(device); 96 | 97 | // Walk the IO Registry tree looking for this devices parent IOUSBDevice. 98 | while( name != "IOUSBDevice" ) 99 | { 100 | kern_result = IORegistryEntryGetParentEntry( device, 101 | kIOServicePlane, 102 | &parent ); 103 | 104 | if(kern_result != KERN_SUCCESS) 105 | { 106 | result = 0; 107 | break; 108 | } 109 | 110 | device = parent; 111 | 112 | name = get_class_name(device); 113 | } 114 | 115 | if(kern_result == KERN_SUCCESS) 116 | result = device; 117 | 118 | return result; 119 | } 120 | 121 | string 122 | get_string_property( io_object_t& device, const char* property ) 123 | { 124 | string property_name; 125 | 126 | if( device ) 127 | { 128 | CFStringRef property_as_cfstring = CFStringCreateWithCString ( 129 | kCFAllocatorDefault, 130 | property, 131 | kCFStringEncodingASCII ); 132 | 133 | CFTypeRef name_as_cfstring = IORegistryEntryCreateCFProperty( 134 | device, 135 | property_as_cfstring, 136 | kCFAllocatorDefault, 137 | 0 ); 138 | 139 | if( name_as_cfstring ) 140 | { 141 | if( CFGetTypeID(name_as_cfstring) == CFStringGetTypeID() ) 142 | property_name = cfstring_to_string( static_cast(name_as_cfstring) ); 143 | 144 | CFRelease(name_as_cfstring); 145 | } 146 | 147 | if(property_as_cfstring) 148 | CFRelease(property_as_cfstring); 149 | } 150 | 151 | return property_name; 152 | } 153 | 154 | uint16_t 155 | get_int_property( io_object_t& device, const char* property ) 156 | { 157 | uint16_t result = 0; 158 | 159 | if( device ) 160 | { 161 | CFStringRef property_as_cfstring = CFStringCreateWithCString ( 162 | kCFAllocatorDefault, 163 | property, 164 | kCFStringEncodingASCII ); 165 | 166 | CFTypeRef number = IORegistryEntryCreateCFProperty( device, 167 | property_as_cfstring, 168 | kCFAllocatorDefault, 169 | 0 ); 170 | 171 | if(property_as_cfstring) 172 | CFRelease(property_as_cfstring); 173 | 174 | if( number ) 175 | { 176 | if( CFGetTypeID(number) == CFNumberGetTypeID() ) 177 | { 178 | bool success = CFNumberGetValue( static_cast(number), 179 | kCFNumberSInt16Type, 180 | &result ); 181 | 182 | if( !success ) 183 | result = 0; 184 | } 185 | 186 | CFRelease(number); 187 | } 188 | 189 | } 190 | 191 | return result; 192 | } 193 | 194 | string rtrim(const string& str) 195 | { 196 | string result = str; 197 | 198 | string whitespace = " \t\f\v\n\r"; 199 | 200 | std::size_t found = result.find_last_not_of(whitespace); 201 | 202 | if (found != std::string::npos) 203 | result.erase(found+1); 204 | else 205 | result.clear(); 206 | 207 | return result; 208 | } 209 | 210 | vector 211 | serial::list_ports(void) 212 | { 213 | vector devices_found; 214 | CFMutableDictionaryRef classes_to_match; 215 | io_iterator_t serial_port_iterator; 216 | io_object_t serial_port; 217 | mach_port_t master_port; 218 | kern_return_t kern_result; 219 | 220 | kern_result = IOMasterPort(MACH_PORT_NULL, &master_port); 221 | 222 | if(kern_result != KERN_SUCCESS) 223 | return devices_found; 224 | 225 | classes_to_match = IOServiceMatching(kIOSerialBSDServiceValue); 226 | 227 | if (classes_to_match == NULL) 228 | return devices_found; 229 | 230 | CFDictionarySetValue( classes_to_match, 231 | CFSTR(kIOSerialBSDTypeKey), 232 | CFSTR(kIOSerialBSDAllTypes) ); 233 | 234 | kern_result = IOServiceGetMatchingServices(master_port, classes_to_match, &serial_port_iterator); 235 | 236 | if (KERN_SUCCESS != kern_result) 237 | return devices_found; 238 | 239 | while ( (serial_port = IOIteratorNext(serial_port_iterator)) ) 240 | { 241 | string device_path = get_device_path( serial_port ); 242 | io_registry_entry_t parent = get_parent_iousb_device( serial_port ); 243 | IOObjectRelease(serial_port); 244 | 245 | if( device_path.empty() ) 246 | continue; 247 | 248 | PortInfo port_info; 249 | port_info.port = device_path; 250 | port_info.description = "n/a"; 251 | port_info.hardware_id = "n/a"; 252 | 253 | string device_name = rtrim( get_string_property( parent, "USB Product Name" ) ); 254 | string vendor_name = rtrim( get_string_property( parent, "USB Vendor Name") ); 255 | string description = rtrim( vendor_name + " " + device_name ); 256 | if( !description.empty() ) 257 | port_info.description = description; 258 | 259 | string serial_number = rtrim(get_string_property( parent, "USB Serial Number" ) ); 260 | uint16_t vendor_id = get_int_property( parent, "idVendor" ); 261 | uint16_t product_id = get_int_property( parent, "idProduct" ); 262 | 263 | if( vendor_id && product_id ) 264 | { 265 | char cstring[HARDWARE_ID_STRING_LENGTH]; 266 | 267 | if(serial_number.empty()) 268 | serial_number = "None"; 269 | 270 | int ret = snprintf( cstring, HARDWARE_ID_STRING_LENGTH, "USB VID:PID=%04x:%04x SNR=%s", 271 | vendor_id, 272 | product_id, 273 | serial_number.c_str() ); 274 | 275 | if( (ret >= 0) && (ret < HARDWARE_ID_STRING_LENGTH) ) 276 | port_info.hardware_id = cstring; 277 | } 278 | 279 | devices_found.push_back(port_info); 280 | } 281 | 282 | IOObjectRelease(serial_port_iterator); 283 | return devices_found; 284 | } 285 | 286 | #endif // defined(__APPLE__) 287 | -------------------------------------------------------------------------------- /ydlidar/sdk/src/impl/list_ports/list_ports_linux.cpp: -------------------------------------------------------------------------------- 1 | #if defined(__linux__) 2 | 3 | /* 4 | * Copyright (c) 2014 Craig Lilley 5 | * This software is made available under the terms of the MIT licence. 6 | * A copy of the licence can be obtained from: 7 | * http://opensource.org/licenses/MIT 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include "serial.h" 27 | 28 | using serial::PortInfo; 29 | using std::istringstream; 30 | using std::ifstream; 31 | using std::getline; 32 | using std::vector; 33 | using std::string; 34 | using std::cout; 35 | using std::endl; 36 | 37 | static vector glob(const vector& patterns); 38 | static string basename(const string& path); 39 | static string dirname(const string& path); 40 | static bool path_exists(const string& path); 41 | static string realpath(const string& path); 42 | static string usb_sysfs_friendly_name(const string& sys_usb_path); 43 | static vector get_sysfs_info(const string& device_path); 44 | static string read_line(const string& file); 45 | static string usb_sysfs_hw_string(const string& sysfs_path); 46 | static string format(const char* format, ...); 47 | 48 | vector 49 | glob(const vector& patterns) 50 | { 51 | vector paths_found; 52 | 53 | if(patterns.size() == 0) 54 | return paths_found; 55 | 56 | glob_t glob_results; 57 | 58 | int glob_retval = glob(patterns[0].c_str(), 0, NULL, &glob_results); 59 | 60 | vector::const_iterator iter = patterns.begin(); 61 | 62 | while(++iter != patterns.end()) 63 | { 64 | glob_retval = glob(iter->c_str(), GLOB_APPEND, NULL, &glob_results); 65 | } 66 | 67 | for(int path_index = 0; path_index < glob_results.gl_pathc; path_index++) 68 | { 69 | paths_found.push_back(glob_results.gl_pathv[path_index]); 70 | } 71 | 72 | globfree(&glob_results); 73 | 74 | return paths_found; 75 | } 76 | 77 | string 78 | basename(const string& path) 79 | { 80 | size_t pos = path.rfind("/"); 81 | 82 | if(pos == std::string::npos) 83 | return path; 84 | 85 | return string(path, pos+1, string::npos); 86 | } 87 | 88 | string 89 | dirname(const string& path) 90 | { 91 | size_t pos = path.rfind("/"); 92 | 93 | if(pos == std::string::npos) 94 | return path; 95 | else if(pos == 0) 96 | return "/"; 97 | 98 | return string(path, 0, pos); 99 | } 100 | 101 | bool 102 | path_exists(const string& path) 103 | { 104 | struct stat sb; 105 | 106 | if( stat(path.c_str(), &sb ) == 0 ) 107 | return true; 108 | 109 | return false; 110 | } 111 | 112 | string 113 | realpath(const string& path) 114 | { 115 | char* real_path = realpath(path.c_str(), NULL); 116 | 117 | string result; 118 | 119 | if(real_path != NULL) 120 | { 121 | result = real_path; 122 | 123 | free(real_path); 124 | } 125 | 126 | return result; 127 | } 128 | 129 | string 130 | usb_sysfs_friendly_name(const string& sys_usb_path) 131 | { 132 | unsigned int device_number = 0; 133 | 134 | istringstream( read_line(sys_usb_path + "/devnum") ) >> device_number; 135 | 136 | string manufacturer = read_line( sys_usb_path + "/manufacturer" ); 137 | 138 | string product = read_line( sys_usb_path + "/product" ); 139 | 140 | string serial = read_line( sys_usb_path + "/serial" ); 141 | 142 | if( manufacturer.empty() && product.empty() && serial.empty() ) 143 | return ""; 144 | 145 | return format("%s %s %s", manufacturer.c_str(), product.c_str(), serial.c_str() ); 146 | } 147 | 148 | vector 149 | get_sysfs_info(const string& device_path) 150 | { 151 | string device_name = basename( device_path ); 152 | 153 | string friendly_name; 154 | 155 | string hardware_id; 156 | 157 | string sys_device_path = format( "/sys/class/tty/%s/device", device_name.c_str() ); 158 | 159 | if( device_name.compare(0,6,"ttyUSB") == 0 ) 160 | { 161 | sys_device_path = dirname( dirname( realpath( sys_device_path ) ) ); 162 | 163 | if( path_exists( sys_device_path ) ) 164 | { 165 | friendly_name = usb_sysfs_friendly_name( sys_device_path ); 166 | 167 | hardware_id = usb_sysfs_hw_string( sys_device_path ); 168 | } 169 | } 170 | else if( device_name.compare(0,6,"ttyACM") == 0 ) 171 | { 172 | sys_device_path = dirname( realpath( sys_device_path ) ); 173 | 174 | if( path_exists( sys_device_path ) ) 175 | { 176 | friendly_name = usb_sysfs_friendly_name( sys_device_path ); 177 | 178 | hardware_id = usb_sysfs_hw_string( sys_device_path ); 179 | } 180 | } 181 | else 182 | { 183 | // Try to read ID string of PCI device 184 | 185 | string sys_id_path = sys_device_path + "/id"; 186 | 187 | if( path_exists( sys_id_path ) ) 188 | hardware_id = read_line( sys_id_path ); 189 | } 190 | 191 | if( friendly_name.empty() ) 192 | friendly_name = device_name; 193 | 194 | if( hardware_id.empty() ) 195 | hardware_id = "n/a"; 196 | 197 | vector result; 198 | result.push_back(friendly_name); 199 | result.push_back(hardware_id); 200 | 201 | return result; 202 | } 203 | 204 | string 205 | read_line(const string& file) 206 | { 207 | ifstream ifs(file.c_str(), ifstream::in); 208 | 209 | string line; 210 | 211 | if(ifs) 212 | { 213 | getline(ifs, line); 214 | } 215 | 216 | return line; 217 | } 218 | 219 | string 220 | format(const char* format, ...) 221 | { 222 | va_list ap; 223 | 224 | size_t buffer_size_bytes = 256; 225 | 226 | string result; 227 | 228 | char* buffer = (char*)malloc(buffer_size_bytes); 229 | 230 | if( buffer == NULL ) 231 | return result; 232 | 233 | bool done = false; 234 | 235 | unsigned int loop_count = 0; 236 | 237 | while(!done) 238 | { 239 | va_start(ap, format); 240 | 241 | int return_value = vsnprintf(buffer, buffer_size_bytes, format, ap); 242 | 243 | if( return_value < 0 ) 244 | { 245 | done = true; 246 | } 247 | else if( return_value >= buffer_size_bytes ) 248 | { 249 | // Realloc and try again. 250 | 251 | buffer_size_bytes = return_value + 1; 252 | 253 | char* new_buffer_ptr = (char*)realloc(buffer, buffer_size_bytes); 254 | 255 | if( new_buffer_ptr == NULL ) 256 | { 257 | done = true; 258 | } 259 | else 260 | { 261 | buffer = new_buffer_ptr; 262 | } 263 | } 264 | else 265 | { 266 | result = buffer; 267 | done = true; 268 | } 269 | 270 | va_end(ap); 271 | 272 | if( ++loop_count > 5 ) 273 | done = true; 274 | } 275 | 276 | free(buffer); 277 | 278 | return result; 279 | } 280 | 281 | string 282 | usb_sysfs_hw_string(const string& sysfs_path) 283 | { 284 | string serial_number = read_line( sysfs_path + "/serial" ); 285 | 286 | if( serial_number.length() > 0 ) 287 | { 288 | serial_number = format( "SNR=%s", serial_number.c_str() ); 289 | } 290 | 291 | string vid = read_line( sysfs_path + "/idVendor" ); 292 | 293 | string pid = read_line( sysfs_path + "/idProduct" ); 294 | 295 | return format("USB VID:PID=%s:%s %s", vid.c_str(), pid.c_str(), serial_number.c_str() ); 296 | } 297 | 298 | vector 299 | serial::list_ports() 300 | { 301 | vector results; 302 | 303 | vector search_globs; 304 | search_globs.push_back("/dev/ttyACM*"); 305 | search_globs.push_back("/dev/ttyS*"); 306 | search_globs.push_back("/dev/ttyUSB*"); 307 | search_globs.push_back("/dev/tty.*"); 308 | search_globs.push_back("/dev/cu.*"); 309 | 310 | vector devices_found = glob( search_globs ); 311 | 312 | vector::iterator iter = devices_found.begin(); 313 | 314 | while( iter != devices_found.end() ) 315 | { 316 | string device = *iter++; 317 | 318 | vector sysfs_info = get_sysfs_info( device ); 319 | 320 | string friendly_name = sysfs_info[0]; 321 | 322 | string hardware_id = sysfs_info[1]; 323 | 324 | PortInfo device_entry; 325 | device_entry.port = device; 326 | device_entry.description = friendly_name; 327 | device_entry.hardware_id = hardware_id; 328 | 329 | results.push_back( device_entry ); 330 | 331 | } 332 | 333 | return results; 334 | } 335 | 336 | #endif // defined(__linux__) 337 | -------------------------------------------------------------------------------- /ydlidar/sdk/src/serial.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #if !defined(_WIN32) && !defined(__OpenBSD__) && !defined(__FreeBSD__) 4 | # include 5 | #endif 6 | 7 | #if defined (__MINGW32__) 8 | # define alloca __builtin_alloca 9 | #endif 10 | 11 | #include "serial.h" 12 | #include "common.h" 13 | 14 | namespace serial { 15 | 16 | using std::min; 17 | using std::numeric_limits; 18 | using std::vector; 19 | using std::size_t; 20 | using std::string; 21 | 22 | using serial::Serial; 23 | using serial::bytesize_t; 24 | using serial::parity_t; 25 | using serial::stopbits_t; 26 | using serial::flowcontrol_t; 27 | 28 | class Serial::ScopedReadLock { 29 | public: 30 | ScopedReadLock(SerialImpl *pimpl) : pimpl_(pimpl) { 31 | this->pimpl_->readLock(); 32 | } 33 | ~ScopedReadLock() { 34 | this->pimpl_->readUnlock(); 35 | } 36 | private: 37 | // Disable copy constructors 38 | ScopedReadLock(const ScopedReadLock&); 39 | const ScopedReadLock& operator=(ScopedReadLock); 40 | 41 | SerialImpl *pimpl_; 42 | }; 43 | 44 | class Serial::ScopedWriteLock { 45 | public: 46 | ScopedWriteLock(SerialImpl *pimpl) : pimpl_(pimpl) { 47 | this->pimpl_->writeLock(); 48 | } 49 | ~ScopedWriteLock() { 50 | this->pimpl_->writeUnlock(); 51 | } 52 | private: 53 | // Disable copy constructors 54 | ScopedWriteLock(const ScopedWriteLock&); 55 | const ScopedWriteLock& operator=(ScopedWriteLock); 56 | SerialImpl *pimpl_; 57 | }; 58 | 59 | Serial::Serial (const string &port, uint32_t baudrate, serial::Timeout timeout, 60 | bytesize_t bytesize, parity_t parity, stopbits_t stopbits, 61 | flowcontrol_t flowcontrol) 62 | : pimpl_(new SerialImpl (port, baudrate, bytesize, parity, 63 | stopbits, flowcontrol)) { 64 | pimpl_->setTimeout(timeout); 65 | } 66 | 67 | Serial::~Serial () { 68 | delete pimpl_; 69 | } 70 | 71 | bool Serial::open () { 72 | return pimpl_->open (); 73 | } 74 | 75 | void Serial::close () { 76 | pimpl_->close (); 77 | } 78 | 79 | bool Serial::isOpen () const { 80 | return pimpl_->isOpen (); 81 | } 82 | 83 | size_t Serial::available () { 84 | return pimpl_->available (); 85 | } 86 | 87 | bool Serial::waitReadable () { 88 | serial::Timeout timeout(pimpl_->getTimeout ()); 89 | return pimpl_->waitReadable(timeout.read_timeout_constant); 90 | } 91 | 92 | void Serial::waitByteTimes (size_t count) { 93 | pimpl_->waitByteTimes(count); 94 | } 95 | 96 | int Serial::waitfordata(size_t data_count, uint32_t timeout, size_t * returned_size) { 97 | return pimpl_->waitfordata(data_count, timeout, returned_size); 98 | } 99 | 100 | size_t Serial::read_ (uint8_t *buffer, size_t size) { 101 | return this->pimpl_->read (buffer, size); 102 | } 103 | 104 | size_t Serial::read (uint8_t *buffer, size_t size) { 105 | ScopedReadLock lock(this->pimpl_); 106 | return this->pimpl_->read (buffer, size); 107 | } 108 | 109 | size_t Serial::read (std::vector &buffer, size_t size) { 110 | ScopedReadLock lock(this->pimpl_); 111 | uint8_t *buffer_ = new uint8_t[size]; 112 | size_t bytes_read = this->pimpl_->read (buffer_, size); 113 | buffer.insert (buffer.end (), buffer_, buffer_+bytes_read); 114 | delete[] buffer_; 115 | return bytes_read; 116 | } 117 | 118 | size_t Serial::read (std::string &buffer, size_t size) { 119 | ScopedReadLock lock(this->pimpl_); 120 | uint8_t *buffer_ = new uint8_t[size]; 121 | size_t bytes_read = this->pimpl_->read (buffer_, size); 122 | buffer.append (reinterpret_cast(buffer_), bytes_read); 123 | delete[] buffer_; 124 | return bytes_read; 125 | } 126 | 127 | string Serial::read (size_t size) { 128 | std::string buffer; 129 | this->read (buffer, size); 130 | return buffer; 131 | } 132 | 133 | size_t Serial::readline (string &buffer, size_t size, string eol) { 134 | ScopedReadLock lock(this->pimpl_); 135 | size_t eol_len = eol.length (); 136 | uint8_t *buffer_ = static_cast (alloca (size * sizeof (uint8_t))); 137 | size_t read_so_far = 0; 138 | while (true) { 139 | size_t bytes_read = this->read_ (buffer_ + read_so_far, 1); 140 | read_so_far += bytes_read; 141 | if (bytes_read == 0) { 142 | break; // Timeout occured on reading 1 byte 143 | } 144 | if (string (reinterpret_cast (buffer_ + read_so_far - eol_len), eol_len) == eol) { 145 | break; // EOL found 146 | } 147 | if (read_so_far == size) { 148 | break; // Reached the maximum read length 149 | } 150 | } 151 | buffer.append(reinterpret_cast (buffer_), read_so_far); 152 | return read_so_far; 153 | } 154 | 155 | string Serial::readline (size_t size, string eol) { 156 | std::string buffer; 157 | this->readline (buffer, size, eol); 158 | return buffer; 159 | } 160 | 161 | vector Serial::readlines (size_t size, string eol) { 162 | ScopedReadLock lock(this->pimpl_); 163 | std::vector lines; 164 | size_t eol_len = eol.length (); 165 | uint8_t *buffer_ = static_cast (alloca (size * sizeof (uint8_t))); 166 | size_t read_so_far = 0; 167 | size_t start_of_line = 0; 168 | while (read_so_far < size) { 169 | size_t bytes_read = this->read_ (buffer_+read_so_far, 1); 170 | read_so_far += bytes_read; 171 | if (bytes_read == 0) { 172 | if (start_of_line != read_so_far) { 173 | lines.push_back ( string (reinterpret_cast (buffer_ + start_of_line), read_so_far - start_of_line)); 174 | } 175 | break; // Timeout occured on reading 1 byte 176 | } 177 | if (string (reinterpret_cast 178 | (buffer_ + read_so_far - eol_len), eol_len) == eol) { 179 | // EOL found 180 | lines.push_back( string(reinterpret_cast (buffer_ + start_of_line), read_so_far - start_of_line)); 181 | start_of_line = read_so_far; 182 | } 183 | if (read_so_far == size) { 184 | if (start_of_line != read_so_far) { 185 | lines.push_back( string(reinterpret_cast (buffer_ + start_of_line), read_so_far - start_of_line)); 186 | } 187 | break; // Reached the maximum read length 188 | } 189 | } 190 | return lines; 191 | } 192 | 193 | size_t Serial::write (const string &data) { 194 | ScopedWriteLock lock(this->pimpl_); 195 | return this->write_ (reinterpret_cast(data.c_str()), data.length()); 196 | } 197 | 198 | size_t Serial::write (const std::vector &data) { 199 | ScopedWriteLock lock(this->pimpl_); 200 | return this->write_ (&data[0], data.size()); 201 | } 202 | 203 | size_t Serial::write (const uint8_t *data, size_t size) { 204 | ScopedWriteLock lock(this->pimpl_); 205 | return this->write_(data, size); 206 | } 207 | 208 | size_t Serial::write_ (const uint8_t *data, size_t length) { 209 | return pimpl_->write (data, length); 210 | } 211 | 212 | void Serial::setPort (const string &port) { 213 | ScopedReadLock rlock(this->pimpl_); 214 | ScopedWriteLock wlock(this->pimpl_); 215 | bool was_open = pimpl_->isOpen (); 216 | if (was_open) close(); 217 | pimpl_->setPort (port); 218 | if (was_open) open (); 219 | } 220 | 221 | string Serial::getPort () const { 222 | return pimpl_->getPort (); 223 | } 224 | 225 | void Serial::setTimeout (serial::Timeout &timeout) { 226 | pimpl_->setTimeout (timeout); 227 | } 228 | 229 | serial::Timeout Serial::getTimeout () const { 230 | return pimpl_->getTimeout (); 231 | } 232 | 233 | bool Serial::setBaudrate (uint32_t baudrate) { 234 | return pimpl_->setBaudrate (baudrate); 235 | } 236 | 237 | uint32_t Serial::getBaudrate () const { 238 | return uint32_t(pimpl_->getBaudrate ()); 239 | } 240 | 241 | bool Serial::setBytesize (bytesize_t bytesize){ 242 | return pimpl_->setBytesize (bytesize); 243 | } 244 | 245 | bytesize_t Serial::getBytesize () const{ 246 | return pimpl_->getBytesize (); 247 | } 248 | 249 | bool Serial::setParity (parity_t parity){ 250 | return pimpl_->setParity (parity); 251 | } 252 | 253 | parity_t Serial::getParity () const{ 254 | return pimpl_->getParity (); 255 | } 256 | 257 | bool Serial::setStopbits (stopbits_t stopbits){ 258 | return pimpl_->setStopbits (stopbits); 259 | } 260 | 261 | stopbits_t Serial::getStopbits () const{ 262 | return pimpl_->getStopbits (); 263 | } 264 | 265 | bool Serial::setFlowcontrol (flowcontrol_t flowcontrol){ 266 | return pimpl_->setFlowcontrol (flowcontrol); 267 | } 268 | 269 | flowcontrol_t Serial::getFlowcontrol () const{ 270 | return pimpl_->getFlowcontrol (); 271 | } 272 | 273 | void Serial::flush (){ 274 | ScopedReadLock rlock(this->pimpl_); 275 | ScopedWriteLock wlock(this->pimpl_); 276 | pimpl_->flush (); 277 | } 278 | 279 | void Serial::flushInput (){ 280 | ScopedReadLock lock(this->pimpl_); 281 | pimpl_->flushInput (); 282 | } 283 | 284 | void Serial::flushOutput (){ 285 | ScopedWriteLock lock(this->pimpl_); 286 | pimpl_->flushOutput (); 287 | } 288 | 289 | void Serial::sendBreak (int duration){ 290 | pimpl_->sendBreak (duration); 291 | } 292 | 293 | bool Serial::setBreak (bool level){ 294 | return pimpl_->setBreak (level); 295 | } 296 | 297 | bool Serial::setRTS (bool level){ 298 | return pimpl_->setRTS (level); 299 | } 300 | 301 | bool Serial::setDTR (bool level){ 302 | return pimpl_->setDTR (level); 303 | } 304 | 305 | bool Serial::waitForChange(){ 306 | return pimpl_->waitForChange(); 307 | } 308 | 309 | bool Serial::getCTS (){ 310 | return pimpl_->getCTS (); 311 | } 312 | 313 | bool Serial::getDSR (){ 314 | return pimpl_->getDSR (); 315 | } 316 | 317 | bool Serial::getRI (){ 318 | return pimpl_->getRI (); 319 | } 320 | 321 | bool Serial::getCD (){ 322 | return pimpl_->getCD (); 323 | } 324 | 325 | uint32_t Serial::getByteTime(){ 326 | return pimpl_->getByteTime(); 327 | } 328 | } -------------------------------------------------------------------------------- /rover_2dnav/rviz/nav.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /GridCells1 10 | - /planner plane1 11 | - /Current goal1 12 | Splitter Ratio: 0.5 13 | Tree Height: 793 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679016 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: LidarLaserScan 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.0299999993 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Alpha: 1 54 | Autocompute Intensity Bounds: true 55 | Autocompute Value Bounds: 56 | Max Value: 0.100000001 57 | Min Value: 0.100000001 58 | Value: true 59 | Axis: Z 60 | Channel Name: intensity 61 | Class: rviz/LaserScan 62 | Color: 255; 255; 255 63 | Color Transformer: AxisColor 64 | Decay Time: 0 65 | Enabled: true 66 | Invert Rainbow: false 67 | Max Color: 255; 255; 255 68 | Max Intensity: 4096 69 | Min Color: 0; 0; 0 70 | Min Intensity: 0 71 | Name: LidarLaserScan 72 | Position Transformer: XYZ 73 | Queue Size: 1000 74 | Selectable: true 75 | Size (Pixels): 5 76 | Size (m): 0.0299999993 77 | Style: Squares 78 | Topic: /scan 79 | Unreliable: false 80 | Use Fixed Frame: true 81 | Use rainbow: true 82 | Value: true 83 | - Alpha: 0.699999988 84 | Class: rviz/Map 85 | Color Scheme: map 86 | Draw Behind: false 87 | Enabled: true 88 | Name: Map 89 | Topic: /map 90 | Unreliable: false 91 | Use Timestamp: false 92 | Value: true 93 | - Alpha: 1 94 | Class: rviz/Polygon 95 | Color: 25; 255; 0 96 | Enabled: true 97 | Name: Polygon 98 | Topic: /move_base/local_costmap/footprint 99 | Unreliable: false 100 | Value: true 101 | - Alpha: 0.100000001 102 | Arrow Length: 0.300000012 103 | Axes Length: 0.300000012 104 | Axes Radius: 0.00999999978 105 | Class: rviz/PoseArray 106 | Color: 255; 25; 0 107 | Enabled: false 108 | Head Length: 0.0700000003 109 | Head Radius: 0.0299999993 110 | Name: PoseArray 111 | Shaft Length: 0.230000004 112 | Shaft Radius: 0.00999999978 113 | Shape: Arrow (Flat) 114 | Topic: /particlecloud 115 | Unreliable: false 116 | Value: false 117 | - Alpha: 1 118 | Class: rviz/GridCells 119 | Color: 25; 255; 0 120 | Enabled: true 121 | Name: GridCells 122 | Topic: "" 123 | Value: true 124 | - Alpha: 0.5 125 | Cell Size: 1 126 | Class: rviz/Grid 127 | Color: 160; 160; 164 128 | Enabled: true 129 | Line Style: 130 | Line Width: 0.0299999993 131 | Value: Lines 132 | Name: Grid 133 | Normal Cell Count: 0 134 | Offset: 135 | X: 0 136 | Y: 0 137 | Z: 0 138 | Plane: XY 139 | Plane Cell Count: 10 140 | Reference Frame: 141 | Value: true 142 | - Alpha: 0.699999988 143 | Class: rviz/Map 144 | Color Scheme: costmap 145 | Draw Behind: false 146 | Enabled: true 147 | Name: local costmap 148 | Topic: /move_base/local_costmap/costmap 149 | Unreliable: false 150 | Use Timestamp: false 151 | Value: true 152 | - Alpha: 0.699999988 153 | Class: rviz/Map 154 | Color Scheme: costmap 155 | Draw Behind: false 156 | Enabled: false 157 | Name: Map 158 | Topic: /move_base/global_costmap/costmap 159 | Unreliable: false 160 | Use Timestamp: false 161 | Value: false 162 | - Class: rviz/TF 163 | Enabled: true 164 | Frame Timeout: 15 165 | Frames: 166 | All Enabled: true 167 | base_link: 168 | Value: true 169 | laser_frame: 170 | Value: true 171 | map: 172 | Value: true 173 | odom: 174 | Value: true 175 | Marker Scale: 1 176 | Name: TF 177 | Show Arrows: true 178 | Show Axes: true 179 | Show Names: true 180 | Tree: 181 | map: 182 | odom: 183 | base_link: 184 | laser_frame: 185 | {} 186 | Update Interval: 0 187 | Value: true 188 | - Alpha: 1 189 | Buffer Length: 1 190 | Class: rviz/Path 191 | Color: 25; 255; 0 192 | Enabled: true 193 | Head Diameter: 0.300000012 194 | Head Length: 0.200000003 195 | Length: 0.300000012 196 | Line Style: Lines 197 | Line Width: 0.0299999993 198 | Name: Path 199 | Offset: 200 | X: 0 201 | Y: 0 202 | Z: 0 203 | Pose Color: 255; 85; 255 204 | Pose Style: None 205 | Radius: 0.0299999993 206 | Shaft Diameter: 0.100000001 207 | Shaft Length: 0.100000001 208 | Topic: /move_base/TrajectoryPlannerROS/global_plan 209 | Unreliable: false 210 | Value: true 211 | - Alpha: 1 212 | Buffer Length: 1 213 | Class: rviz/Path 214 | Color: 255; 0; 0 215 | Enabled: true 216 | Head Diameter: 0.300000012 217 | Head Length: 0.200000003 218 | Length: 0.300000012 219 | Line Style: Lines 220 | Line Width: 0.0299999993 221 | Name: Path 222 | Offset: 223 | X: 0 224 | Y: 0 225 | Z: 0 226 | Pose Color: 255; 85; 255 227 | Pose Style: None 228 | Radius: 0.0299999993 229 | Shaft Diameter: 0.100000001 230 | Shaft Length: 0.100000001 231 | Topic: /move_base/TrajectoryPlannerROS/local_plan 232 | Unreliable: false 233 | Value: true 234 | - Alpha: 1 235 | Buffer Length: 1 236 | Class: rviz/Path 237 | Color: 255; 85; 255 238 | Enabled: true 239 | Head Diameter: 0.300000012 240 | Head Length: 0.200000003 241 | Length: 0.300000012 242 | Line Style: Lines 243 | Line Width: 0.0299999993 244 | Name: planner plane 245 | Offset: 246 | X: 0 247 | Y: 0 248 | Z: 0 249 | Pose Color: 255; 85; 255 250 | Pose Style: None 251 | Radius: 0.0299999993 252 | Shaft Diameter: 0.100000001 253 | Shaft Length: 0.100000001 254 | Topic: /move_base/NavfnROS/plan 255 | Unreliable: false 256 | Value: true 257 | - Alpha: 1 258 | Axes Length: 1 259 | Axes Radius: 0.100000001 260 | Class: rviz/Pose 261 | Color: 170; 170; 0 262 | Enabled: true 263 | Head Length: 0.300000012 264 | Head Radius: 0.100000001 265 | Name: Current goal 266 | Shaft Length: 1 267 | Shaft Radius: 0.0500000007 268 | Shape: Arrow 269 | Topic: /move_base/current_goal 270 | Unreliable: false 271 | Value: true 272 | Enabled: true 273 | Global Options: 274 | Background Color: 48; 48; 48 275 | Default Light: true 276 | Fixed Frame: map 277 | Frame Rate: 30 278 | Name: root 279 | Tools: 280 | - Class: rviz/MoveCamera 281 | - Class: rviz/Interact 282 | Hide Inactive Objects: true 283 | - Class: rviz/Select 284 | - Class: rviz/SetInitialPose 285 | Topic: /initialpose 286 | - Class: rviz/SetGoal 287 | Topic: /move_base_simple/goal 288 | Value: true 289 | Views: 290 | Current: 291 | Class: rviz/Orbit 292 | Distance: 24.9623127 293 | Enable Stereo Rendering: 294 | Stereo Eye Separation: 0.0599999987 295 | Stereo Focal Distance: 1 296 | Swap Stereo Eyes: false 297 | Value: false 298 | Focal Point: 299 | X: -0.0344972014 300 | Y: 0.0658859983 301 | Z: 0.148092002 302 | Focal Shape Fixed Size: true 303 | Focal Shape Size: 0.0500000007 304 | Invert Z Axis: false 305 | Name: Current View 306 | Near Clip Distance: 0.00999999978 307 | Pitch: 1.3797965 308 | Target Frame: 309 | Value: Orbit (rviz) 310 | Yaw: 5.25856876 311 | Saved: ~ 312 | Window Geometry: 313 | Displays: 314 | collapsed: false 315 | Height: 1031 316 | Hide Left Dock: false 317 | Hide Right Dock: false 318 | QMainWindow State: 000000ff00000000fd000000040000000000000195000003a8fc0200000006fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000003a8000000d700fffffffb0000000a0049006d006100670065000000028d000000c60000000000000000000000010000010b00000294fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003300000294000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005e5000003a800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 319 | Selection: 320 | collapsed: false 321 | Time: 322 | collapsed: false 323 | Tool Properties: 324 | collapsed: false 325 | Views: 326 | collapsed: false 327 | Width: 1920 328 | X: 0 329 | Y: 27 330 | -------------------------------------------------------------------------------- /ydlidar/test/laser_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * YDLIDAR SYSTEM 3 | * 4 | * Copyright 2015 - 2018 EAI TEAM 5 | * http://www.ydlidar.com 6 | * 7 | */ 8 | 9 | #include "laser_test.h" 10 | using namespace serial; 11 | using namespace ydlidar; 12 | Lasertest::DEVICE_STATE Lasertest::device_state_ = CLOSED; 13 | static int nodes_count = 720; 14 | static float each_angle = 0.5; 15 | 16 | Lasertest::Lasertest() 17 | :publish_freq_(40), 18 | scan_no_(0) { 19 | port_ = "/dev/ttyACM0"; 20 | baudrate_ = 115200; 21 | angle_min_ = -180; 22 | angle_max_ = 180; 23 | intensities_ = false; 24 | inverted = false; 25 | 26 | } 27 | 28 | Lasertest::~Lasertest() { 29 | } 30 | 31 | void Lasertest::setPort(std::string port) 32 | { 33 | port_ = port; 34 | } 35 | 36 | void Lasertest::setBaudrate(int baud) 37 | { 38 | baudrate_ = baud; 39 | } 40 | 41 | void Lasertest::setIntensities(bool intensities) 42 | { 43 | intensities_ = intensities; 44 | } 45 | 46 | 47 | void Lasertest::run() { 48 | Open(); 49 | Start(); 50 | } 51 | 52 | void Lasertest::closeApp(int signo){ 53 | device_state_ = CLOSED; 54 | signal(signo, SIG_DFL); 55 | } 56 | 57 | /** Returns true if the device is connected & operative */ 58 | bool Lasertest::getDeviceInfo() 59 | { 60 | if (!YDlidarDriver::singleton()){ 61 | return false; 62 | } 63 | 64 | device_info devinfo; 65 | if (YDlidarDriver::singleton()->getDeviceInfo(devinfo) !=RESULT_OK){ 66 | fprintf(stderr,"[YDLIDAR] get DeviceInfo Error\n" ); 67 | return false; 68 | } 69 | 70 | sampling_rate _rate; 71 | std::string model; 72 | switch(devinfo.model){ 73 | case 1: 74 | model="F4"; 75 | break; 76 | case 2: 77 | model="T1"; 78 | break; 79 | case 3: 80 | model="F2"; 81 | break; 82 | case 4: 83 | model="S4"; 84 | break; 85 | case 5: 86 | model="G4"; 87 | YDlidarDriver::singleton()->getSamplingRate(_rate); 88 | switch(_rate.rate){ 89 | case 0: 90 | break; 91 | case 1: 92 | nodes_count = 1440; 93 | each_angle = 0.25; 94 | break; 95 | case 2: 96 | nodes_count = 1440; 97 | each_angle = 0.25; 98 | break; 99 | } 100 | break; 101 | case 6: 102 | model="X4"; 103 | break; 104 | case 8: 105 | model="F4Pro"; 106 | YDlidarDriver::singleton()->getSamplingRate(_rate); 107 | switch(_rate.rate){ 108 | case 0: 109 | break; 110 | case 1: 111 | nodes_count = 1440; 112 | each_angle = 0.25; 113 | break; 114 | } 115 | break; 116 | case 9: 117 | model="G4C"; 118 | break; 119 | default: 120 | model = "Unknown"; 121 | break; 122 | } 123 | 124 | unsigned int maxv = (unsigned int)(devinfo.firmware_version>>8); 125 | unsigned int midv = (unsigned int)(devinfo.firmware_version&0xff)/10; 126 | unsigned int minv = (unsigned int)(devinfo.firmware_version&0xff)%10; 127 | 128 | printf("[YDLIDAR] Connection established in [%s]:\n" 129 | "Firmware version: %u.%u.%u\n" 130 | "Hardware version: %u\n" 131 | "Model: %s\n" 132 | "Serial: ", 133 | port_.c_str(), 134 | maxv, 135 | midv, 136 | minv, 137 | (unsigned int)devinfo.hardware_version, 138 | model.c_str()); 139 | 140 | for (int i=0;i<16;i++){ 141 | printf("%01X",devinfo.serialnum[i]&0xff); 142 | } 143 | printf("\n"); 144 | return true; 145 | 146 | } 147 | 148 | /** Returns true if the device is connected & operative */ 149 | bool Lasertest::getDeviceHealth() 150 | { 151 | if (!YDlidarDriver::singleton()) return false; 152 | 153 | result_t op_result; 154 | device_health healthinfo; 155 | 156 | op_result = YDlidarDriver::singleton()->getHealth(healthinfo); 157 | if (op_result == RESULT_OK) { 158 | fprintf(stdout,"[YDLIDAR] running correctly ! The health status:%s\n" ,healthinfo.status==0? "good":"bad"); 159 | if (healthinfo.status == 2) { 160 | fprintf(stderr, "Error, [YDLIDAR] internal error detected. Please reboot the device to retry.\n"); 161 | return false; 162 | } else { 163 | return true; 164 | } 165 | 166 | } else { 167 | fprintf(stderr, "Error, cannot retrieve YDLIDAR health code: %x\n", op_result); 168 | return false; 169 | } 170 | } 171 | 172 | void Lasertest::Open() { 173 | try { 174 | if(!YDlidarDriver::singleton()){ 175 | YDlidarDriver::initDriver(); 176 | } 177 | result_t op_result = YDlidarDriver::singleton()->connect(port_.c_str(), (uint32_t)baudrate_); 178 | if (op_result != RESULT_OK) { 179 | fprintf(stdout,"open Lidar is failed! Exit!! ......\n"); 180 | return; 181 | } 182 | 183 | signal(SIGINT, closeApp); 184 | signal(SIGTERM, closeApp); 185 | 186 | bool ret = getDeviceHealth(); 187 | if(!getDeviceInfo()&&!ret){ 188 | YDlidarDriver::singleton()->disconnect(); 189 | YDlidarDriver::done(); 190 | return; 191 | } 192 | 193 | result_t ans=YDlidarDriver::singleton()->startScan(); 194 | if(ans != RESULT_OK){ 195 | fprintf(stdout,"start Lidar is failed! Exit!! ......\n"); 196 | YDlidarDriver::singleton()->disconnect(); 197 | YDlidarDriver::done(); 198 | return; 199 | } 200 | 201 | YDlidarDriver::singleton()->setIntensities(intensities_); 202 | fprintf(stdout,"Device opened successfully.\n"); 203 | device_state_ = OPENED; 204 | } catch (std::exception &e) { 205 | Close(); 206 | fprintf(stdout,"can't open laser\n "); 207 | } 208 | } 209 | 210 | void Lasertest::Start() { 211 | if(device_state_ !=OPENED|| !YDlidarDriver::singleton()){ 212 | return; 213 | } 214 | node_info all_nodes[nodes_count]; 215 | memset(all_nodes, 0, nodes_count*sizeof(node_info)); 216 | fprintf(stdout,"Now YDLIDAR is scanning.\n"); 217 | device_state_ = RUNNING; 218 | 219 | double scan_duration; 220 | result_t op_result; 221 | 222 | while (device_state_ == RUNNING) { 223 | try { 224 | node_info nodes[nodes_count]; 225 | size_t count = _countof(nodes); 226 | uint64_t start_scan_time = getms(); 227 | op_result = YDlidarDriver::singleton()->grabScanData(nodes, count); 228 | uint64_t end_scan_time = getms(); 229 | scan_duration = (end_scan_time - start_scan_time); 230 | 231 | if (op_result == RESULT_OK) { 232 | op_result = YDlidarDriver::singleton()->ascendScanData(nodes, count); 233 | if (op_result == RESULT_OK) { 234 | memset(all_nodes, 0, nodes_count*sizeof(node_info)); 235 | for(size_t i =0 ; i < count; i++) { 236 | if (nodes[i].distance_q2 != 0) { 237 | float angle = (float)((nodes[i].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f); 238 | int inter =(int)( angle / each_angle ); 239 | float angle_pre = angle - inter * each_angle; 240 | float angle_next = (inter+1) * each_angle - angle; 241 | if(angle_pre < angle_next){ 242 | if(inter < nodes_count) 243 | all_nodes[inter]=nodes[i]; 244 | }else{ 245 | if(inter < nodes_count-1) 246 | all_nodes[inter+1]=nodes[i]; 247 | 248 | } 249 | } 250 | 251 | } 252 | publicScanData(all_nodes,start_scan_time,scan_duration,nodes_count,angle_min_,angle_max_,inverted); 253 | 254 | } 255 | 256 | }else if(op_result == RESULT_FAIL){ 257 | } 258 | 259 | } catch (std::exception& e) { 260 | fprintf(stderr,"Exception thrown while starting YDLIDAR.\n "); 261 | break; 262 | } catch (...) { 263 | fprintf(stderr,"Exception thrown while trying to get scan: \n"); 264 | break; 265 | } 266 | } 267 | 268 | Close(); 269 | } 270 | 271 | void Lasertest::Stop() { 272 | device_state_ = OPENED; 273 | } 274 | 275 | void Lasertest::Close() { 276 | try { 277 | YDlidarDriver::singleton()->disconnect(); 278 | YDlidarDriver::done(); 279 | device_state_ = CLOSED; 280 | exit(0); 281 | } catch (std::exception &e) { 282 | return; 283 | } 284 | } 285 | 286 | std::vector Lasertest::split(const std::string &s, char delim) { 287 | std::vector elems; 288 | std::stringstream ss(s); 289 | std::string number; 290 | while(std::getline(ss, number, delim)) { 291 | elems.push_back(atoi(number.c_str())); 292 | } 293 | return elems; 294 | } 295 | 296 | void Lasertest::publicScanData(node_info *nodes, uint64_t start,double scan_time, size_t node_count, float angle_min,float angle_max, bool reverse_data) { 297 | fprintf(stdout,"publicScanData: %lud , %i\n",start, (int)node_count); 298 | 299 | int counts = node_count*((angle_max-angle_min)/360.0f); 300 | int angle_start = 180+angle_min; 301 | int node_start = node_count*(angle_start/360.0f); 302 | 303 | LaserScan scan; 304 | scan.system_time_stamp = start; 305 | scan.self_time_stamp = start; 306 | 307 | float radian_min = DEG2RAD(angle_min); 308 | float radian_max = DEG2RAD(angle_max); 309 | 310 | 311 | scan.config.min_angle = radian_min; 312 | scan.config.max_angle = radian_max; 313 | scan.config.ang_increment = (radian_max - radian_min) / (double)counts; 314 | scan.config.time_increment = scan_time / (double)counts; 315 | scan.config.scan_time = scan_time; 316 | scan.config.min_angle = 0.1; 317 | scan.config.max_range = 15.; 318 | 319 | scan.ranges.resize(counts); 320 | scan.intensities.resize(counts); 321 | 322 | float range = 0.0; 323 | float intensity = 0.0; 324 | int index = 0; 325 | for (size_t i = 0; i < node_count; i++) { 326 | range = (float)nodes[i].distance_q2/4.0f/1000; 327 | intensity = (float)(nodes[i].sync_quality >> 2); 328 | 329 | if(i 4 | #include 5 | #include "locker.h" 6 | #include "serial.h" 7 | #include "thread.h" 8 | 9 | #if !defined(__cplusplus) 10 | #ifndef __cplusplus 11 | #error "The YDLIDAR SDK requires a C++ compiler to be built" 12 | #endif 13 | #endif 14 | #if !defined(_countof) 15 | #define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0])) 16 | #endif 17 | 18 | #define LIDAR_CMD_STOP 0x65 19 | #define LIDAR_CMD_SCAN 0x60 20 | #define LIDAR_CMD_FORCE_SCAN 0x61 21 | #define LIDAR_CMD_RESET 0x80 22 | #define LIDAR_CMD_FORCE_STOP 0x00 23 | #define LIDAR_CMD_GET_EAI 0x55 24 | #define LIDAR_CMD_GET_DEVICE_INFO 0x90 25 | #define LIDAR_CMD_GET_DEVICE_HEALTH 0x92 26 | #define LIDAR_ANS_TYPE_DEVINFO 0x4 27 | #define LIDAR_ANS_TYPE_DEVHEALTH 0x6 28 | #define LIDAR_CMD_SYNC_BYTE 0xA5 29 | #define LIDAR_CMDFLAG_HAS_PAYLOAD 0x80 30 | #define LIDAR_ANS_SYNC_BYTE1 0xA5 31 | #define LIDAR_ANS_SYNC_BYTE2 0x5A 32 | #define LIDAR_ANS_TYPE_MEASUREMENT 0x81 33 | #define LIDAR_RESP_MEASUREMENT_SYNCBIT (0x1<<0) 34 | #define LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT 2 35 | #define LIDAR_RESP_MEASUREMENT_CHECKBIT (0x1<<0) 36 | #define LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT 1 37 | 38 | #define LIDAR_CMD_RUN_POSITIVE 0x06 39 | #define LIDAR_CMD_RUN_INVERSION 0x07 40 | #define LIDAR_CMD_SET_AIMSPEED_ADDMIC 0x09 41 | #define LIDAR_CMD_SET_AIMSPEED_DISMIC 0x0A 42 | #define LIDAR_CMD_SET_AIMSPEED_ADD 0x0B 43 | #define LIDAR_CMD_SET_AIMSPEED_DIS 0x0C 44 | #define LIDAR_CMD_GET_AIMSPEED 0x0D 45 | 46 | #define LIDAR_CMD_SET_SAMPLING_RATE 0xD0 47 | #define LIDAR_CMD_GET_SAMPLING_RATE 0xD1 48 | #define LIDAR_STATUS_OK 0x0 49 | #define LIDAR_STATUS_WARNING 0x1 50 | #define LIDAR_STATUS_ERROR 0x2 51 | 52 | #define LIDAR_CMD_ENABLE_LOW_POWER 0x01 53 | #define LIDAR_CMD_DISABLE_LOW_POWER 0x02 54 | #define LIDAR_CMD_STATE_MODEL_MOTOR 0x05 55 | #define LIDAR_CMD_ENABLE_CONST_FREQ 0x0E 56 | #define LIDAR_CMD_DISABLE_CONST_FREQ 0x0F 57 | 58 | #define LIDAR_CMD_SAVE_SET_EXPOSURE 0x94 59 | #define LIDAR_CMD_SET_LOW_EXPOSURE 0x95 60 | #define LIDAR_CMD_ADD_EXPOSURE 0x96 61 | #define LIDAR_CMD_DIS_EXPOSURE 0x97 62 | 63 | #define LIDAR_CMD_SET_HEART_BEAT 0xD9 64 | #define LIDAR_CMD_SET_SETPOINTSFORONERINGFLAG 0xae 65 | 66 | #define PackageSampleMaxLngth 0x100 67 | typedef enum { 68 | CT_Normal = 0, 69 | CT_RingStart = 1, 70 | CT_Tail, 71 | }CT; 72 | #define Node_Default_Quality (10<<2) 73 | #define Node_Sync 1 74 | #define Node_NotSync 2 75 | #define PackagePaidBytes 10 76 | #define PH 0x55AA 77 | 78 | #if defined(_WIN32) 79 | #pragma pack(1) 80 | #endif 81 | 82 | struct node_info { 83 | uint8_t sync_quality; 84 | uint16_t angle_q6_checkbit; 85 | uint16_t distance_q2; 86 | uint64_t stamp; 87 | } __attribute__((packed)) ; 88 | 89 | struct PackageNode { 90 | uint8_t PakageSampleQuality; 91 | uint16_t PakageSampleDistance; 92 | }__attribute__((packed)); 93 | 94 | struct node_package { 95 | uint16_t package_Head; 96 | uint8_t package_CT; 97 | uint8_t nowPackageNum; 98 | uint16_t packageFirstSampleAngle; 99 | uint16_t packageLastSampleAngle; 100 | uint16_t checkSum; 101 | PackageNode packageSample[PackageSampleMaxLngth]; 102 | } __attribute__((packed)) ; 103 | 104 | struct node_packages { 105 | uint16_t package_Head; 106 | uint8_t package_CT; 107 | uint8_t nowPackageNum; 108 | uint16_t packageFirstSampleAngle; 109 | uint16_t packageLastSampleAngle; 110 | uint16_t checkSum; 111 | uint16_t packageSampleDistance[PackageSampleMaxLngth]; 112 | } __attribute__((packed)) ; 113 | 114 | 115 | struct device_info{ 116 | uint8_t model; 117 | uint16_t firmware_version; 118 | uint8_t hardware_version; 119 | uint8_t serialnum[16]; 120 | } __attribute__((packed)) ; 121 | 122 | struct device_health { 123 | uint8_t status; 124 | uint16_t error_code; 125 | } __attribute__((packed)) ; 126 | 127 | struct sampling_rate { 128 | uint8_t rate; 129 | } __attribute__((packed)) ; 130 | 131 | struct scan_frequency { 132 | uint32_t frequency; 133 | } __attribute__((packed)) ; 134 | 135 | struct scan_rotation { 136 | uint8_t rotation; 137 | } __attribute__((packed)) ; 138 | 139 | struct scan_exposure { 140 | uint8_t exposure; 141 | } __attribute__((packed)) ; 142 | 143 | struct scan_heart_beat { 144 | uint8_t enable; 145 | } __attribute__((packed)); 146 | 147 | struct scan_points { 148 | uint8_t flag; 149 | } __attribute__((packed)) ; 150 | 151 | struct function_state { 152 | uint8_t state; 153 | } __attribute__((packed)) ; 154 | 155 | struct cmd_packet { 156 | uint8_t syncByte; 157 | uint8_t cmd_flag; 158 | uint8_t size; 159 | uint8_t data; 160 | } __attribute__((packed)) ; 161 | 162 | struct lidar_ans_header { 163 | uint8_t syncByte1; 164 | uint8_t syncByte2; 165 | uint32_t size:30; 166 | uint32_t subType:2; 167 | uint8_t type; 168 | } __attribute__((packed)); 169 | 170 | struct scanDot { 171 | uint8_t quality; 172 | float angle; 173 | float dist; 174 | }; 175 | 176 | 177 | //! A struct for returning configuration from the YDLIDAR 178 | struct LaserConfig { 179 | //! Start angle for the laser scan [rad]. 0 is forward and angles are measured clockwise when viewing YDLIDAR from the top. 180 | float min_angle; 181 | //! Stop angle for the laser scan [rad]. 0 is forward and angles are measured clockwise when viewing YDLIDAR from the top. 182 | float max_angle; 183 | //! Scan resolution [rad]. 184 | float ang_increment; 185 | //! Scan resoltuion [s] 186 | float time_increment; 187 | //! Time between scans 188 | float scan_time; 189 | //! Minimum range [m] 190 | float min_range; 191 | //! Maximum range [m] 192 | float max_range; 193 | //! Range Resolution [m] 194 | float range_res; 195 | }; 196 | 197 | 198 | //! A struct for returning laser readings from the YDLIDAR 199 | struct LaserScan { 200 | //! Array of ranges 201 | std::vector ranges; 202 | //! Array of intensities 203 | std::vector intensities; 204 | //! Self reported time stamp in nanoseconds 205 | uint64_t self_time_stamp; 206 | //! System time when first range was measured in nanoseconds 207 | uint64_t system_time_stamp; 208 | //! Configuration of scan 209 | LaserConfig config; 210 | }; 211 | 212 | using namespace std; 213 | using namespace serial; 214 | 215 | namespace ydlidar{ 216 | 217 | class YDlidarDriver 218 | { 219 | public: 220 | static YDlidarDriver* singleton(){ 221 | return _impl; 222 | } 223 | static void initDriver(){ 224 | _impl = new YDlidarDriver; 225 | } 226 | static void done(){ 227 | if(_impl){ 228 | delete _impl; 229 | _impl = NULL; 230 | } 231 | } 232 | 233 | result_t connect(const char * port_path, uint32_t baudrate); 234 | void disconnect(); 235 | static std::string getSDKVersion(); 236 | const bool isscanning(); 237 | const bool isconnected(); 238 | void setIntensities(const bool isintensities); 239 | const bool getHeartBeat(); 240 | void setHeartBeat(const bool enable); 241 | result_t sendHeartBeat(); 242 | result_t getHealth(device_health & health, uint32_t timeout = DEFAULT_TIMEOUT); 243 | result_t getDeviceInfo(device_info & info, uint32_t timeout = DEFAULT_TIMEOUT); 244 | result_t startScan(bool force = false, uint32_t timeout = DEFAULT_TIMEOUT) ; 245 | result_t stop(); 246 | result_t grabScanData(node_info * nodebuffer, size_t & count, uint32_t timeout = DEFAULT_TIMEOUT) ; 247 | result_t ascendScanData(node_info * nodebuffer, size_t count); 248 | void simpleScanData(std::vector * scan_data , node_info *buffer, size_t count); 249 | 250 | result_t reset(uint32_t timeout = DEFAULT_TIMEOUT); 251 | result_t startMotor(); 252 | result_t stopMotor(); 253 | result_t getScanFrequency(scan_frequency & frequency, uint32_t timeout = DEFAULT_TIMEOUT); 254 | result_t setScanFrequencyAdd(scan_frequency & frequency, uint32_t timeout = DEFAULT_TIMEOUT); 255 | result_t setScanFrequencyDis(scan_frequency & frequency, uint32_t timeout = DEFAULT_TIMEOUT); 256 | result_t setScanFrequencyAddMic(scan_frequency & frequency, uint32_t timeout = DEFAULT_TIMEOUT); 257 | result_t setScanFrequencyDisMic(scan_frequency & frequency, uint32_t timeout = DEFAULT_TIMEOUT); 258 | result_t getSamplingRate(sampling_rate & rate, uint32_t timeout = DEFAULT_TIMEOUT); 259 | result_t setSamplingRate(sampling_rate & rate, uint32_t timeout = DEFAULT_TIMEOUT); 260 | result_t setRotationPositive(scan_rotation & rotation, uint32_t timeout = DEFAULT_TIMEOUT); 261 | result_t setRotationInversion(scan_rotation & rotation, uint32_t timeout = DEFAULT_TIMEOUT); 262 | 263 | result_t enableLowerPower(function_state & state, uint32_t timeout = DEFAULT_TIMEOUT); 264 | result_t disableLowerPower(function_state & state, uint32_t timeout = DEFAULT_TIMEOUT); 265 | result_t getMotorState(function_state & state, uint32_t timeout = DEFAULT_TIMEOUT); 266 | result_t enableConstFreq(function_state & state, uint32_t timeout = DEFAULT_TIMEOUT); 267 | result_t disableConstFreq(function_state & state, uint32_t timeout = DEFAULT_TIMEOUT); 268 | 269 | result_t setSaveLowExposure(scan_exposure& low_exposure, uint32_t timeout = DEFAULT_TIMEOUT); 270 | result_t setLowExposure(scan_exposure& low_exposure, uint32_t timeout = DEFAULT_TIMEOUT); 271 | result_t setLowExposureAdd(scan_exposure & exposure, uint32_t timeout = DEFAULT_TIMEOUT); 272 | result_t setLowExposurerDis(scan_exposure & exposure, uint32_t timeout = DEFAULT_TIMEOUT); 273 | result_t setScanHeartbeat(scan_heart_beat& beat,uint32_t timeout = DEFAULT_TIMEOUT); 274 | result_t setPointsForOneRingFlag(scan_points& points,uint32_t timeout = DEFAULT_TIMEOUT); 275 | 276 | protected: 277 | YDlidarDriver(); 278 | virtual ~YDlidarDriver(); 279 | 280 | result_t createThread(); 281 | result_t waitPackage(node_info * node, uint32_t timeout = DEFAULT_TIMEOUT); 282 | result_t waitScanData(node_info * nodebuffer, size_t & count, uint32_t timeout = DEFAULT_TIMEOUT); 283 | int cacheScanData(); 284 | result_t sendCommand(uint8_t cmd, const void * payload = NULL, size_t payloadsize = 0); 285 | result_t waitResponseHeader(lidar_ans_header * header, uint32_t timeout = DEFAULT_TIMEOUT); 286 | result_t waitForData(size_t data_count,uint32_t timeout = -1, size_t * returned_size = NULL); 287 | result_t getData(uint8_t * data, size_t size); 288 | result_t sendData(const uint8_t * data, size_t size); 289 | void disableDataGrabbing(); 290 | void setDTR(); 291 | void clearDTR(); 292 | 293 | 294 | public: 295 | std::atomic isConnected; 296 | std::atomic isScanning; 297 | std::atomic isHeartbeat; 298 | 299 | enum { 300 | DEFAULT_TIMEOUT = 2000, 301 | DEFAULT_HEART_BEAT = 1000, 302 | MAX_SCAN_NODES = 2048, 303 | }; 304 | enum { 305 | YDLIDAR_F4=1, 306 | YDLIDAR_T1=2, 307 | YDLIDAR_F2=3, 308 | YDLIDAR_S4=4, 309 | YDLIDAR_G4=5, 310 | YDLIDAR_X4=6, 311 | }; 312 | node_info scan_node_buf[2048]; 313 | size_t scan_node_count; 314 | Event _dataEvent; 315 | Locker _lock; 316 | Thread _thread; 317 | 318 | private: 319 | static int PackageSampleBytes; 320 | static YDlidarDriver* _impl; 321 | serial::Serial *_serial; 322 | bool m_intensities; 323 | int _sampling_rate; 324 | int model; 325 | uint32_t _baudrate; 326 | bool isSupportMotorCtrl; 327 | uint64_t m_ns; 328 | uint64_t m_calc_ns; 329 | uint32_t m_pointTime; 330 | uint32_t trans_delay; 331 | 332 | }; 333 | } 334 | 335 | #endif // YDLIDAR_DRIVER_H 336 | -------------------------------------------------------------------------------- /ydlidar/sdk/include/CYdLidar.cpp: -------------------------------------------------------------------------------- 1 | #include "CYdLidar.h" 2 | #include "common.h" 3 | #include 4 | 5 | #ifndef __countof 6 | #define __countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0])) 7 | #endif 8 | 9 | 10 | using namespace std; 11 | using namespace ydlidar; 12 | using namespace impl; 13 | 14 | int CYdLidar::NODE_COUNTS =720; 15 | double CYdLidar::EACH_ANGLE = 0.5; 16 | /*------------------------------------------------------------- 17 | Constructor 18 | -------------------------------------------------------------*/ 19 | CYdLidar::CYdLidar() : 20 | m_com_port(""), 21 | m_com_port_baudrate(115200), 22 | m_intensity(false), 23 | max_range(16.0), 24 | min_range(0.08) 25 | { 26 | } 27 | 28 | /*------------------------------------------------------------- 29 | ~CFlashLidar 30 | -------------------------------------------------------------*/ 31 | CYdLidar::~CYdLidar() 32 | { 33 | disconnecting(); 34 | } 35 | 36 | void CYdLidar::disconnecting() 37 | { 38 | if(YDlidarDriver::singleton()){ 39 | YDlidarDriver::singleton()->disconnect(); 40 | YDlidarDriver::done(); 41 | } 42 | } 43 | 44 | /*------------------------------------------------------------- 45 | doProcessSimple 46 | -------------------------------------------------------------*/ 47 | bool CYdLidar::doProcessSimple(LaserScan &outscan, bool &hardwareError){ 48 | hardwareError = false; 49 | 50 | // Bound? 51 | if (!checkCOMMs()) 52 | { 53 | hardwareError = true; 54 | return false; 55 | } 56 | 57 | node_info nodes[NODE_COUNTS]; 58 | size_t count = __countof(nodes); 59 | 60 | // Scan: 61 | const uint64_t tim_scan_start = getus(); 62 | result_t op_result = YDlidarDriver::singleton()->grabScanData(nodes, count); 63 | const uint64_t tim_scan_end = getus(); 64 | 65 | 66 | const bool compensate = true; 67 | 68 | // Fill in scan data: 69 | if (op_result == RESULT_OK) 70 | { 71 | op_result = YDlidarDriver::singleton()->ascendScanData(nodes, count); 72 | 73 | //同步后的时间 74 | if(nodes[0].stamp > 0){ 75 | start_scan_time.sec = nodes[0].stamp/1000000000; 76 | start_scan_time.nsec = (nodes[0].stamp%1000000000); 77 | } 78 | const double scan_time = tim_scan_end - tim_scan_start; 79 | 80 | 81 | if (op_result == RESULT_OK) 82 | { 83 | if(compensate){ 84 | const int angle_compensate_nodes_count = NODE_COUNTS; 85 | node_info angle_compensate_nodes[NODE_COUNTS]; 86 | memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(node_info)); 87 | 88 | unsigned int i = 0; 89 | for( ; i < count; i++) { 90 | if (nodes[i].distance_q2 != 0) { 91 | float angle = (float)((nodes[i].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f); 92 | int inter =(int)( angle / EACH_ANGLE ); 93 | float angle_pre = angle - inter * EACH_ANGLE; 94 | float angle_next = (inter+1) * EACH_ANGLE - angle; 95 | if(angle_pre < angle_next){ 96 | if(inter < NODE_COUNTS) 97 | angle_compensate_nodes[inter]=nodes[i]; 98 | }else{ 99 | if(inter < NODE_COUNTS -1) 100 | angle_compensate_nodes[inter+1]=nodes[i]; 101 | } 102 | } 103 | } 104 | 105 | LaserScan scan_msg; 106 | scan_msg.ranges.resize(NODE_COUNTS); 107 | scan_msg.intensities.resize(NODE_COUNTS); 108 | float range = 0.0; 109 | float intensity = 0.0; 110 | int index = 0; 111 | 112 | 113 | for (size_t i = 0; i < NODE_COUNTS; i++) { 114 | range = (float)angle_compensate_nodes[i].distance_q2/4.0f/1000; 115 | intensity = (float)(angle_compensate_nodes[i].sync_quality >> LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT); 116 | 117 | if(i max_range|| range < min_range){ 124 | range = 0.0; 125 | } 126 | 127 | 128 | 129 | int pos = index ; 130 | if(0<= pos && pos < NODE_COUNTS){ 131 | scan_msg.ranges[pos] = range; 132 | scan_msg.intensities[pos] = intensity; 133 | } 134 | } 135 | 136 | scan_msg.system_time_stamp = tim_scan_start; 137 | scan_msg.self_time_stamp = tim_scan_start; 138 | scan_msg.config.min_angle = -PI; 139 | scan_msg.config.max_angle = PI; 140 | scan_msg.config.ang_increment = (scan_msg.config.max_angle - scan_msg.config.min_angle) / (double)NODE_COUNTS; 141 | scan_msg.config.time_increment = scan_time / (double)NODE_COUNTS; 142 | scan_msg.config.scan_time = scan_time; 143 | scan_msg.config.min_angle = min_range; 144 | scan_msg.config.max_range = max_range; 145 | outscan = scan_msg; 146 | return true; 147 | 148 | 149 | 150 | 151 | } 152 | 153 | } 154 | 155 | } 156 | else 157 | { 158 | if (op_result==RESULT_FAIL) 159 | { 160 | // Error? Retry connection 161 | //this->disconnect(); 162 | } 163 | } 164 | 165 | return false; 166 | 167 | } 168 | 169 | 170 | /*------------------------------------------------------------- 171 | turnOn 172 | -------------------------------------------------------------*/ 173 | bool CYdLidar::turnOn() 174 | { 175 | bool ret = checkCOMMs(); 176 | if (ret && YDlidarDriver::singleton()){ 177 | YDlidarDriver::singleton()->startMotor(); 178 | } 179 | 180 | return ret; 181 | } 182 | 183 | /*------------------------------------------------------------- 184 | turnOff 185 | -------------------------------------------------------------*/ 186 | bool CYdLidar::turnOff() 187 | { 188 | if (YDlidarDriver::singleton()) { 189 | YDlidarDriver::singleton()->stop(); 190 | YDlidarDriver::singleton()->stopMotor(); 191 | } 192 | return true; 193 | } 194 | 195 | /** Returns true if the device is connected & operative */ 196 | bool CYdLidar::getDeviceHealth() const 197 | { 198 | if (!YDlidarDriver::singleton()) return false; 199 | 200 | result_t op_result; 201 | device_health healthinfo; 202 | 203 | op_result = YDlidarDriver::singleton()->getHealth(healthinfo); 204 | if (op_result == RESULT_OK) { 205 | printf("Yd Lidar running correctly ! The health status: %s\n", (int)healthinfo.status==0?"good":"bad"); 206 | 207 | if (healthinfo.status == 2) { 208 | fprintf(stderr, "Error, Yd Lidar internal error detected. Please reboot the device to retry.\n"); 209 | return false; 210 | } else { 211 | return true; 212 | } 213 | 214 | } else { 215 | fprintf(stderr, "Error, cannot retrieve Yd Lidar health code: %x\n", op_result); 216 | return false; 217 | } 218 | } 219 | 220 | bool CYdLidar::getDeviceDeviceInfo() const{ 221 | if (!YDlidarDriver::singleton()) return false; 222 | 223 | 224 | device_info devinfo; 225 | if (YDlidarDriver::singleton()->getDeviceInfo(devinfo) !=0){ 226 | fprintf(stderr, "get DeviceInfo Error\n" ); 227 | return false; 228 | } 229 | std::string model; 230 | switch(devinfo.model){ 231 | case 1: 232 | model="F4"; 233 | break; 234 | case 2: 235 | model="T1"; 236 | break; 237 | case 3: 238 | model="F2"; 239 | break; 240 | case 4: 241 | model="S4"; 242 | break; 243 | case 5: 244 | { 245 | model="G4"; 246 | sampling_rate _rate; 247 | YDlidarDriver::singleton()->getSamplingRate(_rate); 248 | 249 | switch(_rate.rate){ 250 | case 0: 251 | fprintf(stdout,"Current Sampling Rate : 4K\n"); 252 | break; 253 | case 1: 254 | NODE_COUNTS = 1440; 255 | EACH_ANGLE = 0.25; 256 | fprintf(stdout,"Current Sampling Rate : 8K\n"); 257 | break; 258 | case 2: 259 | NODE_COUNTS = 1440; 260 | EACH_ANGLE = 0.25; 261 | fprintf(stdout,"Current Sampling Rate : 9K\n"); 262 | break; 263 | } 264 | 265 | } 266 | break; 267 | case 6: 268 | model="X4"; 269 | break; 270 | default: 271 | model = "Unknown"; 272 | } 273 | 274 | unsigned int maxv = (unsigned int)(devinfo.firmware_version>>8); 275 | unsigned int midv = (unsigned int)(devinfo.firmware_version&0xff)/10; 276 | unsigned int minv = (unsigned int)(devinfo.firmware_version&0xff)%10; 277 | if(midv==0){ 278 | midv = minv; 279 | minv = 0; 280 | } 281 | 282 | printf("[YDLIDAR] Connection established in [%s]:\n" 283 | "Firmware version: %u.%u.%u\n" 284 | "Hardware version: %u\n" 285 | "Model: %s\n" 286 | "Serial: ", 287 | m_com_port.c_str(), 288 | maxv, 289 | midv, 290 | minv, 291 | (unsigned int)devinfo.hardware_version, 292 | model.c_str()); 293 | 294 | for (int i=0;i<16;i++) 295 | printf("%01X",devinfo.serialnum[i]&0xff); 296 | printf("\n"); 297 | 298 | return true; 299 | 300 | 301 | } 302 | 303 | 304 | 305 | /*------------------------------------------------------------- 306 | checkCOMMs 307 | -------------------------------------------------------------*/ 308 | bool CYdLidar::checkCOMMs() 309 | { 310 | if (YDlidarDriver::singleton()) 311 | return true; 312 | 313 | // create the driver instance 314 | YDlidarDriver::initDriver(); 315 | if(!YDlidarDriver::singleton()){ 316 | fprintf(stderr, "Create Driver fail\n"); 317 | return false; 318 | 319 | } 320 | 321 | 322 | // Is it COMX, X>4? -> "\\.\COMX" 323 | if (m_com_port.size()>=3) 324 | { 325 | if ( tolower( m_com_port[0]) =='c' && tolower( m_com_port[1]) =='o' && tolower( m_com_port[2]) =='m' ) 326 | { 327 | // Need to add "\\.\"? 328 | if (m_com_port.size()>4 || m_com_port[3]>'4') 329 | m_com_port = std::string("\\\\.\\") + m_com_port; 330 | } 331 | } 332 | 333 | // make connection... 334 | result_t op_result = YDlidarDriver::singleton()->connect(m_com_port.c_str(), m_com_port_baudrate); 335 | if (op_result != RESULT_OK) 336 | { 337 | fprintf(stderr, "[CYdLidar] Error, cannot bind to the specified serial port %s\n", m_com_port.c_str() ); 338 | return false; 339 | } 340 | 341 | 342 | 343 | // check health: 344 | bool ret = getDeviceHealth() 345 | 346 | if(!getDeviceDeviceInfo()&&!ret) 347 | return false; 348 | 349 | YDlidarDriver::singleton()->setIntensities(m_intensity); 350 | // start scan... 351 | int s_result= YDlidarDriver::singleton()->startScan(); 352 | if (s_result !=RESULT_OK) 353 | { 354 | fprintf(stderr, "[CYdLidar] Error starting scanning mode: %x\n", s_result); 355 | return false; 356 | } 357 | 358 | 359 | return true; 360 | } 361 | 362 | /*------------------------------------------------------------- 363 | initialize 364 | -------------------------------------------------------------*/ 365 | void CYdLidar::initialize() 366 | { 367 | bool ret = true; 368 | if (!checkCOMMs()){ 369 | fprintf(stderr,"[CYdLidar::initialize] Error initializing YDLIDAR scanner."); 370 | 371 | } 372 | if (!turnOn()){ 373 | fprintf(stderr,"[CYdLidar::initialize] Error initializing YDLIDAR scanner."); 374 | 375 | } 376 | 377 | } 378 | 379 | 380 | void CYdLidar::setSerialPort(const std::string &port_name) 381 | { 382 | if (YDlidarDriver::singleton()){ 383 | fprintf(stderr,"Can't change serial port while connected!"); 384 | return; 385 | } 386 | m_com_port = port_name; 387 | 388 | } 389 | 390 | void CYdLidar::setSerialBaud(const uint32_t baudrate) 391 | { 392 | if (YDlidarDriver::singleton()){ 393 | fprintf(stderr,"Can't change serial baudrate while connected!"); 394 | return ; 395 | } 396 | m_com_port_baudrate = baudrate; 397 | 398 | } 399 | 400 | void CYdLidar::setIntensities( bool intensity) 401 | { 402 | if (YDlidarDriver::singleton()){ 403 | fprintf(stderr,"Can't change intensity while connected!"); 404 | return ; 405 | } 406 | m_intensity = intensity; 407 | 408 | } 409 | 410 | void CYdLidar::setMaxRange(const float range) 411 | { 412 | max_range = range; 413 | } 414 | 415 | 416 | void CYdLidar::setMinRange(const float range) 417 | { 418 | min_range = range; 419 | } 420 | 421 | -------------------------------------------------------------------------------- /ydlidar/sdk/include/serial.h: -------------------------------------------------------------------------------- 1 | #ifndef SERIAL_H 2 | #define SERIAL_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "v8stdint.h" 10 | 11 | namespace serial { 12 | 13 | /*! 14 | * Enumeration defines the possible bytesizes for the serial port. 15 | */ 16 | typedef enum { 17 | fivebits = 5, 18 | sixbits = 6, 19 | sevenbits = 7, 20 | eightbits = 8 21 | } bytesize_t; 22 | 23 | /*! 24 | * Enumeration defines the possible parity types for the serial port. 25 | */ 26 | typedef enum { 27 | parity_none = 0, 28 | parity_odd = 1, 29 | parity_even = 2, 30 | parity_mark = 3, 31 | parity_space = 4 32 | } parity_t; 33 | 34 | /*! 35 | * Enumeration defines the possible stopbit types for the serial port. 36 | */ 37 | typedef enum { 38 | stopbits_one = 1, 39 | stopbits_two = 2, 40 | stopbits_one_point_five 41 | } stopbits_t; 42 | 43 | /*! 44 | * Enumeration defines the possible flowcontrol types for the serial port. 45 | */ 46 | typedef enum { 47 | flowcontrol_none = 0, 48 | flowcontrol_software, 49 | flowcontrol_hardware 50 | } flowcontrol_t; 51 | 52 | /*! 53 | * Structure for setting the timeout of the serial port, times are 54 | * in milliseconds. 55 | * 56 | * In order to disable the interbyte timeout, set it to Timeout::max(). 57 | */ 58 | struct Timeout { 59 | #ifdef max 60 | # undef max 61 | #endif 62 | static uint32_t max() {return std::numeric_limits::max();} 63 | /*! 64 | * Convenience function to generate Timeout structs using a 65 | * single absolute timeout. 66 | * 67 | * \param timeout A long that defines the time in milliseconds until a 68 | * timeout occurs after a call to read or write is made. 69 | * 70 | * \return Timeout struct that represents this simple timeout provided. 71 | */ 72 | static Timeout simpleTimeout(uint32_t timeout) { 73 | return Timeout(max(), timeout, 0, timeout, 0); 74 | } 75 | 76 | /*! Number of milliseconds between bytes received to timeout on. */ 77 | uint32_t inter_byte_timeout; 78 | /*! A constant number of milliseconds to wait after calling read. */ 79 | uint32_t read_timeout_constant; 80 | /*! A multiplier against the number of requested bytes to wait after 81 | * calling read. 82 | */ 83 | uint32_t read_timeout_multiplier; 84 | /*! A constant number of milliseconds to wait after calling write. */ 85 | uint32_t write_timeout_constant; 86 | /*! A multiplier against the number of requested bytes to wait after 87 | * calling write. 88 | */ 89 | uint32_t write_timeout_multiplier; 90 | 91 | explicit Timeout (uint32_t inter_byte_timeout_=0, 92 | uint32_t read_timeout_constant_=0, 93 | uint32_t read_timeout_multiplier_=0, 94 | uint32_t write_timeout_constant_=0, 95 | uint32_t write_timeout_multiplier_=0) 96 | : inter_byte_timeout(inter_byte_timeout_), 97 | read_timeout_constant(read_timeout_constant_), 98 | read_timeout_multiplier(read_timeout_multiplier_), 99 | write_timeout_constant(write_timeout_constant_), 100 | write_timeout_multiplier(write_timeout_multiplier_) 101 | {} 102 | }; 103 | 104 | /*! 105 | * Class that provides a portable serial port interface. 106 | */ 107 | class Serial { 108 | public: 109 | /*! 110 | * Creates a Serial object and opens the port if a port is specified, 111 | * otherwise it remains closed until serial::Serial::open is called. 112 | * 113 | * \param port A std::string containing the address of the serial port, 114 | * which would be something like 'COM1' on Windows and '/dev/ttyS0' 115 | * on Linux. 116 | * 117 | * \param baudrate An unsigned 32-bit integer that represents the baudrate 118 | * 119 | * \param timeout A serial::Timeout struct that defines the timeout 120 | * conditions for the serial port. \see serial::Timeout 121 | * 122 | * \param bytesize Size of each byte in the serial transmission of data, 123 | * default is eightbits, possible values are: fivebits, sixbits, sevenbits, 124 | * eightbits 125 | * 126 | * \param parity Method of parity, default is parity_none, possible values 127 | * are: parity_none, parity_odd, parity_even 128 | * 129 | * \param stopbits Number of stop bits used, default is stopbits_one, 130 | * possible values are: stopbits_one, stopbits_one_point_five, stopbits_two 131 | * 132 | * \param flowcontrol Type of flowcontrol used, default is 133 | * flowcontrol_none, possible values are: flowcontrol_none, 134 | * flowcontrol_software, flowcontrol_hardware 135 | * 136 | * \throw serial::PortNotOpenedException 137 | * \throw serial::IOException 138 | * \throw std::invalid_argument 139 | */ 140 | Serial (const std::string &port = "", 141 | uint32_t baudrate = 9600, 142 | Timeout timeout = Timeout(), 143 | bytesize_t bytesize = eightbits, 144 | parity_t parity = parity_none, 145 | stopbits_t stopbits = stopbits_one, 146 | flowcontrol_t flowcontrol = flowcontrol_none); 147 | 148 | /*! Destructor */ 149 | virtual ~Serial (); 150 | 151 | /*! 152 | * Opens the serial port as long as the port is set and the port isn't 153 | * already open. 154 | * 155 | * If the port is provided to the constructor then an explicit call to open 156 | * is not needed. 157 | * 158 | * \see Serial::Serial 159 | * \return Returns true if the port is open, false otherwise. 160 | */ 161 | bool open (); 162 | 163 | /*! Gets the open status of the serial port. 164 | * 165 | * \return Returns true if the port is open, false otherwise. 166 | */ 167 | bool isOpen () const; 168 | 169 | /*! Closes the serial port. */ 170 | void close (); 171 | 172 | /*! Return the number of characters in the buffer. */ 173 | size_t available (); 174 | 175 | /*! Block until there is serial data to read or read_timeout_constant 176 | * number of milliseconds have elapsed. The return value is true when 177 | * the function exits with the port in a readable state, false otherwise 178 | * (due to timeout or select interruption). */ 179 | bool waitReadable (); 180 | 181 | /*! Block for a period of time corresponding to the transmission time of 182 | * count characters at present serial settings. This may be used in con- 183 | * junction with waitReadable to read larger blocks of data from the 184 | * port. */ 185 | void waitByteTimes (size_t count); 186 | 187 | 188 | int waitfordata(size_t data_count, uint32_t timeout, size_t * returned_size); 189 | 190 | /*! Read a given amount of bytes from the serial port into a given buffer. 191 | * 192 | * The read function will return in one of three cases: 193 | * * The number of requested bytes was read. 194 | * * In this case the number of bytes requested will match the size_t 195 | * returned by read. 196 | * * A timeout occurred, in this case the number of bytes read will not 197 | * match the amount requested, but no exception will be thrown. One of 198 | * two possible timeouts occurred: 199 | * * The inter byte timeout expired, this means that number of 200 | * milliseconds elapsed between receiving bytes from the serial port 201 | * exceeded the inter byte timeout. 202 | * * The total timeout expired, which is calculated by multiplying the 203 | * read timeout multiplier by the number of requested bytes and then 204 | * added to the read timeout constant. If that total number of 205 | * milliseconds elapses after the initial call to read a timeout will 206 | * occur. 207 | * * An exception occurred, in this case an actual exception will be thrown. 208 | * 209 | * \param buffer An uint8_t array of at least the requested size. 210 | * \param size A size_t defining how many bytes to be read. 211 | * 212 | * \return A size_t representing the number of bytes read as a result of the 213 | * call to read. 214 | * 215 | */ 216 | size_t read (uint8_t *buffer, size_t size); 217 | 218 | /*! Read a given amount of bytes from the serial port into a give buffer. 219 | * 220 | * \param buffer A reference to a std::vector of uint8_t. 221 | * \param size A size_t defining how many bytes to be read. 222 | * 223 | * \return A size_t representing the number of bytes read as a result of the 224 | * call to read. 225 | * 226 | */ 227 | size_t read (std::vector &buffer, size_t size = 1); 228 | 229 | /*! Read a given amount of bytes from the serial port into a give buffer. 230 | * 231 | * \param buffer A reference to a std::string. 232 | * \param size A size_t defining how many bytes to be read. 233 | * 234 | * \return A size_t representing the number of bytes read as a result of the 235 | * call to read. 236 | * 237 | */ 238 | size_t read (std::string &buffer, size_t size = 1); 239 | 240 | /*! Read a given amount of bytes from the serial port and return a string 241 | * containing the data. 242 | * 243 | * \param size A size_t defining how many bytes to be read. 244 | * 245 | * \return A std::string containing the data read from the port. 246 | * 247 | */ 248 | std::string read (size_t size = 1); 249 | 250 | /*! Reads in a line or until a given delimiter has been processed. 251 | * 252 | * Reads from the serial port until a single line has been read. 253 | * 254 | * \param buffer A std::string reference used to store the data. 255 | * \param size A maximum length of a line, defaults to 65536 (2^16) 256 | * \param eol A string to match against for the EOL. 257 | * 258 | * \return A size_t representing the number of bytes read. 259 | * 260 | */ 261 | size_t readline (std::string &buffer, size_t size = 65536, std::string eol = "\n"); 262 | 263 | /*! Reads in a line or until a given delimiter has been processed. 264 | * 265 | * Reads from the serial port until a single line has been read. 266 | * 267 | * \param size A maximum length of a line, defaults to 65536 (2^16) 268 | * \param eol A string to match against for the EOL. 269 | * 270 | * \return A std::string containing the line. 271 | * 272 | */ 273 | std::string readline (size_t size = 65536, std::string eol = "\n"); 274 | 275 | /*! Reads in multiple lines until the serial port times out. 276 | * 277 | * This requires a timeout > 0 before it can be run. It will read until a 278 | * timeout occurs and return a list of strings. 279 | * 280 | * \param size A maximum length of combined lines, defaults to 65536 (2^16) 281 | * 282 | * \param eol A string to match against for the EOL. 283 | * 284 | * \return A vector containing the lines. 285 | * 286 | */ 287 | std::vector readlines (size_t size = 65536, std::string eol = "\n"); 288 | 289 | /*! Write a string to the serial port. 290 | * 291 | * \param data A const reference containing the data to be written 292 | * to the serial port. 293 | * 294 | * \param size A size_t that indicates how many bytes should be written from 295 | * the given data buffer. 296 | * 297 | * \return A size_t representing the number of bytes actually written to 298 | * the serial port. 299 | * 300 | * \throw serial::PortNotOpenedException 301 | * \throw serial::SerialException 302 | * \throw serial::IOException 303 | */ 304 | size_t write (const uint8_t *data, size_t size); 305 | 306 | /*! Write a string to the serial port. 307 | * 308 | * \param data A const reference containing the data to be written 309 | * to the serial port. 310 | * 311 | * \return A size_t representing the number of bytes actually written to 312 | * the serial port. 313 | * 314 | */ 315 | size_t write (const std::vector &data); 316 | 317 | /*! Write a string to the serial port. 318 | * 319 | * \param data A const reference containing the data to be written 320 | * to the serial port. 321 | * 322 | * \return A size_t representing the number of bytes actually written to 323 | * the serial port. 324 | * 325 | */ 326 | size_t write (const std::string &data); 327 | 328 | /*! Sets the serial port identifier. 329 | * 330 | * \param port A const std::string reference containing the address of the 331 | * serial port, which would be something like 'COM1' on Windows and 332 | * '/dev/ttyS0' on Linux. 333 | * 334 | * \throw std::invalid_argument 335 | */ 336 | void setPort (const std::string &port); 337 | 338 | /*! Gets the serial port identifier. 339 | * 340 | * \see Serial::setPort 341 | * 342 | * \throw std::invalid_argument 343 | */ 344 | std::string getPort () const; 345 | 346 | /*! Sets the timeout for reads and writes using the Timeout struct. 347 | * 348 | * There are two timeout conditions described here: 349 | * * The inter byte timeout: 350 | * * The inter_byte_timeout component of serial::Timeout defines the 351 | * maximum amount of time, in milliseconds, between receiving bytes on 352 | * the serial port that can pass before a timeout occurs. Setting this 353 | * to zero will prevent inter byte timeouts from occurring. 354 | * * Total time timeout: 355 | * * The constant and multiplier component of this timeout condition, 356 | * for both read and write, are defined in serial::Timeout. This 357 | * timeout occurs if the total time since the read or write call was 358 | * made exceeds the specified time in milliseconds. 359 | * * The limit is defined by multiplying the multiplier component by the 360 | * number of requested bytes and adding that product to the constant 361 | * component. In this way if you want a read call, for example, to 362 | * timeout after exactly one second regardless of the number of bytes 363 | * you asked for then set the read_timeout_constant component of 364 | * serial::Timeout to 1000 and the read_timeout_multiplier to zero. 365 | * This timeout condition can be used in conjunction with the inter 366 | * byte timeout condition with out any problems, timeout will simply 367 | * occur when one of the two timeout conditions is met. This allows 368 | * users to have maximum control over the trade-off between 369 | * responsiveness and efficiency. 370 | * 371 | * Read and write functions will return in one of three cases. When the 372 | * reading or writing is complete, when a timeout occurs, or when an 373 | * exception occurs. 374 | * 375 | * A timeout of 0 enables non-blocking mode. 376 | * 377 | * \param timeout A serial::Timeout struct containing the inter byte 378 | * timeout, and the read and write timeout constants and multipliers. 379 | * 380 | * \see serial::Timeout 381 | */ 382 | void setTimeout (Timeout &timeout); 383 | 384 | /*! Sets the timeout for reads and writes. */ 385 | void setTimeout (uint32_t inter_byte_timeout, uint32_t read_timeout_constant, 386 | uint32_t read_timeout_multiplier, uint32_t write_timeout_constant, 387 | uint32_t write_timeout_multiplier) 388 | { 389 | Timeout timeout(inter_byte_timeout, read_timeout_constant, 390 | read_timeout_multiplier, write_timeout_constant, 391 | write_timeout_multiplier); 392 | return setTimeout(timeout); 393 | } 394 | 395 | /*! Gets the timeout for reads in seconds. 396 | * 397 | * \return A Timeout struct containing the inter_byte_timeout, and read 398 | * and write timeout constants and multipliers. 399 | * 400 | * \see Serial::setTimeout 401 | */ 402 | Timeout getTimeout () const; 403 | 404 | /*! Sets the baudrate for the serial port. 405 | * 406 | * Possible baudrates depends on the system but some safe baudrates include: 407 | * 110, 300, 600, 1200, 2400, 4800, 9600, 14400, 19200, 28800, 38400, 56000, 408 | * 57600, 115200 409 | * Some other baudrates that are supported by some comports: 410 | * 128000, 153600, 230400, 256000, 460800, 921600 411 | * 412 | * \param baudrate An integer that sets the baud rate for the serial port. 413 | * 414 | */ 415 | bool setBaudrate (uint32_t baudrate); 416 | 417 | /*! Gets the baudrate for the serial port. 418 | * 419 | * \return An integer that sets the baud rate for the serial port. 420 | * 421 | * \see Serial::setBaudrate 422 | * 423 | * \ 424 | */ 425 | uint32_t getBaudrate () const; 426 | 427 | /*! Sets the bytesize for the serial port. 428 | * 429 | * \param bytesize Size of each byte in the serial transmission of data, 430 | * default is eightbits, possible values are: fivebits, sixbits, sevenbits, 431 | * eightbits 432 | * 433 | * \ 434 | */ 435 | bool setBytesize (bytesize_t bytesize); 436 | 437 | /*! Gets the bytesize for the serial port. 438 | * 439 | * \see Serial::setBytesize 440 | * 441 | * \ 442 | */ 443 | bytesize_t getBytesize () const; 444 | 445 | /*! Sets the parity for the serial port. 446 | * 447 | * \param parity Method of parity, default is parity_none, possible values 448 | * are: parity_none, parity_odd, parity_even 449 | * 450 | * \ 451 | */ 452 | bool setParity (parity_t parity); 453 | 454 | /*! Gets the parity for the serial port. 455 | * 456 | * \see Serial::setParity 457 | * 458 | * \ 459 | */ 460 | parity_t getParity () const; 461 | 462 | /*! Sets the stopbits for the serial port. 463 | * 464 | * \param stopbits Number of stop bits used, default is stopbits_one, 465 | * possible values are: stopbits_one, stopbits_one_point_five, stopbits_two 466 | * 467 | * \ 468 | */ 469 | bool setStopbits (stopbits_t stopbits); 470 | 471 | /*! Gets the stopbits for the serial port. 472 | * 473 | * \see Serial::setStopbits 474 | * 475 | * \ 476 | */ 477 | stopbits_t getStopbits () const; 478 | 479 | /*! Sets the flow control for the serial port. 480 | * 481 | * \param flowcontrol Type of flowcontrol used, default is flowcontrol_none, 482 | * possible values are: flowcontrol_none, flowcontrol_software, 483 | * flowcontrol_hardware 484 | * 485 | * \ 486 | */ 487 | bool setFlowcontrol (flowcontrol_t flowcontrol); 488 | 489 | /*! Gets the flow control for the serial port. 490 | * 491 | * \see Serial::setFlowcontrol 492 | * 493 | * \ 494 | */ 495 | flowcontrol_t getFlowcontrol () const; 496 | 497 | /*! Flush the input and output buffers */ 498 | void flush (); 499 | 500 | /*! Flush only the input buffer */ 501 | void flushInput (); 502 | 503 | /*! Flush only the output buffer */ 504 | void flushOutput (); 505 | 506 | /*! Sends the RS-232 break signal. See tcsendbreak(3). */ 507 | void sendBreak (int duration); 508 | 509 | /*! Set the break condition to a given level. Defaults to true. */ 510 | bool setBreak (bool level = true); 511 | 512 | /*! Set the RTS handshaking line to the given level. Defaults to true. */ 513 | bool setRTS (bool level = true); 514 | 515 | /*! Set the DTR handshaking line to the given level. Defaults to true. */ 516 | bool setDTR (bool level = true); 517 | 518 | /*! 519 | * Blocks until CTS, DSR, RI, CD changes or something interrupts it. 520 | * 521 | * Can throw an exception if an error occurs while waiting. 522 | * You can check the status of CTS, DSR, RI, and CD once this returns. 523 | * Uses TIOCMIWAIT via ioctl if available (mostly only on Linux) with a 524 | * resolution of less than +-1ms and as good as +-0.2ms. Otherwise a 525 | * polling method is used which can give +-2ms. 526 | * 527 | * \return Returns true if one of the lines changed, false if something else 528 | * occurred. 529 | * 530 | */ 531 | bool waitForChange (); 532 | 533 | /*! Returns the current status of the CTS line. */ 534 | bool getCTS (); 535 | 536 | /*! Returns the current status of the DSR line. */ 537 | bool getDSR (); 538 | 539 | /*! Returns the current status of the RI line. */ 540 | bool getRI (); 541 | 542 | /*! Returns the current status of the CD line. */ 543 | bool getCD (); 544 | 545 | /*! Returns the singal byte time. */ 546 | uint32_t getByteTime(); 547 | 548 | private: 549 | // Disable copy constructors 550 | Serial(const Serial&); 551 | Serial& operator=(const Serial&); 552 | 553 | // Pimpl idiom, d_pointer 554 | class SerialImpl; 555 | SerialImpl *pimpl_; 556 | 557 | // Scoped Lock Classes 558 | class ScopedReadLock; 559 | class ScopedWriteLock; 560 | 561 | // Read common function 562 | size_t read_ (uint8_t *buffer, size_t size); 563 | // Write common function 564 | size_t write_ (const uint8_t *data, size_t length); 565 | }; 566 | 567 | 568 | /*! 569 | * Structure that describes a serial device. 570 | */ 571 | struct PortInfo { 572 | 573 | /*! Address of the serial port (this can be passed to the constructor of Serial). */ 574 | std::string port; 575 | 576 | /*! Human readable description of serial device if available. */ 577 | std::string description; 578 | 579 | /*! Hardware ID (e.g. VID:PID of USB serial devices) or "n/a" if not available. */ 580 | std::string hardware_id; 581 | 582 | }; 583 | 584 | /* Lists the serial ports available on the system 585 | * 586 | * Returns a vector of available serial ports, each represented 587 | * by a serial::PortInfo data structure: 588 | * 589 | * \return vector of serial::PortInfo. 590 | */ 591 | //std::vector list_ports(); 592 | 593 | } // namespace serial 594 | 595 | #endif 596 | --------------------------------------------------------------------------------