├── .gitignore ├── README.md ├── benewake_ce30c ├── CMakeLists.txt ├── README ├── include │ └── ce30c_driver │ │ ├── base.h │ │ ├── ce30_sdk.h │ │ └── undistort.h ├── launch │ ├── benewake_ce30c.launch │ ├── benewake_ce30c_change_ip.launch │ ├── benewake_ce30c_new_address.launch │ ├── benewake_ce30c_tif.launch │ ├── hector_slam.launch │ ├── pointcloud_2_laserscan.launch │ └── test_tf.launch ├── libbw_ce30v2.0.so ├── package.xml ├── readme ├── rosbag │ └── ce30_02.bag.7z ├── rviz │ └── pointcloud.rviz ├── src │ └── ce30_cloud.cpp └── windows_software │ └── CE30-C_PointCoud_Viewer_v1.0.0.7z ├── donkey_car ├── CMakeLists.txt ├── launch │ ├── avoidance_demo.launch │ ├── blob_chase_demo.launch │ └── keyboard_demo.launch ├── package.xml └── src │ ├── chase_the_ball.py │ ├── low_level_control.py │ ├── low_level_control_blob_chase.py │ ├── low_level_control_with_sonar.py │ └── obstacle_avoid_sonar.py ├── donkey_sonar ├── 3dprint │ └── TfMini Bracket.STL ├── CMakeLists.txt ├── launch │ └── sonar_array.launch ├── package.xml ├── src │ ├── sonar.py │ └── sonar_array.py └── test │ └── test_sonar.py ├── laser_scanner_tfmini ├── 3dprint │ └── TfMini Bracket.STL ├── CMakeLists.txt ├── package.xml └── src │ ├── readme.txt │ ├── ros_tfmini_laser_scanner.py │ ├── servoblaster.py │ ├── tfmini.py │ └── tfmini_servo_scanner.py ├── opencv ├── CMakeLists.txt ├── README.txt ├── __init__.py ├── include │ ├── __init__.py │ ├── blob.jpg │ ├── blob2.jpg │ ├── blob3.jpg │ ├── blob_detector.py │ └── range_detector.py ├── launch │ └── camerav2_320x240.launch ├── package.xml └── src │ ├── __init__.py │ ├── find_ball.py │ └── test_cv_bridge.py ├── ros-i2cpwmboard ├── .gitlab-ci.yml ├── CMakeLists.txt ├── README.md ├── doc │ ├── Doxyfile │ ├── doxygen.css │ ├── footer.html │ ├── header.html │ ├── html │ │ ├── arrowdown.png │ │ ├── arrowright.png │ │ ├── bc_s.png │ │ ├── bdwn.png │ │ ├── closed.png │ │ ├── dir_68267d1309a1af8e8297ef4c3efbcdba.html │ │ ├── doc.png │ │ ├── doxygen.css │ │ ├── doxygen.png │ │ ├── files.html │ │ ├── folderclosed.png │ │ ├── folderopen.png │ │ ├── globals.html │ │ ├── globals_func.html │ │ ├── group___services.html │ │ ├── group___topics.html │ │ ├── i2cpwm__controller_8cpp.html │ │ ├── i2cpwm__controller_8cpp_source.html │ │ ├── index.html │ │ ├── jquery.js │ │ ├── modules.html │ │ ├── nav_f.png │ │ ├── nav_g.png │ │ ├── nav_h.png │ │ ├── open.png │ │ ├── splitbar.png │ │ ├── styles.css │ │ ├── sync_off.png │ │ ├── sync_on.png │ │ ├── tab_a.png │ │ ├── tab_b.png │ │ ├── tab_h.png │ │ ├── tab_s.png │ │ └── tabs.css │ └── styles.css ├── launch │ └── i2cpwm_node.launch ├── msg │ ├── Position.msg │ ├── PositionArray.msg │ ├── Servo.msg │ ├── ServoArray.msg │ ├── ServoConfig.msg │ └── ServoConfigArray.msg ├── package.xml ├── src │ └── i2cpwm_controller.cpp └── srv │ ├── DriveMode.srv │ ├── IntValue.srv │ ├── ServosConfig.srv │ └── StopServos.srv └── test_pub_sub ├── CMakeLists.txt ├── msg └── test_custom_msg.msg ├── package.xml └── src ├── test_publisher.py ├── test_publisher_custom.py ├── test_subscriber.py └── test_subscriber_custom.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ros_tutorials 2 | Tutorials with ROS: Find videos on my channel https://www.youtube.com/channel/UC2aPsByptP3HXobjXgsXQsA?view_as=subscriber 3 | 4 | ------- Setup the ROS_MASTER_URI ----------- 5 | If ROS Master runs on the Raspberry Pi, you need to set 6 | export ROS_MASTER_URI=http://ubiquityrobot.local:11311 7 | 8 | In case you are running ROS Master on your local host, set 9 | export ROS_MASTER_URI=http://localhost:11311 10 | 11 | To speedup the process you can edit ~.bashrc with: 12 | 13 | source ~/catkin_ws/devel/setup.bash 14 | export ROS_IP=`ifconfig | grep -Eo 'inet (addr:)?([0-9]*\.){3}[0-9]*' | grep -Eo '([0-9]*\.){3}[0-9]*' | grep -v '127.0.0.1'` 15 | 16 | export ROS_MASTER_URI=http://ubiquityrobot.local:11311 17 | or 18 | export ROS_MASTER_URI=http://localhost:11311 19 | -------------------------------------------------------------------------------- /benewake_ce30c/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ce30c_driver) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | # add_definitions(-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 | roscpp 12 | rospy 13 | sensor_msgs 14 | std_msgs 15 | ) 16 | 17 | find_package(PCL REQUIRED) 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 | # sensor_msgs# std_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 you 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 ce30c_driver 110 | # CATKIN_DEPENDS roscpp rospy sensor_msgs std_msgs 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(include) 121 | include_directories( 122 | include 123 | ${catkin_INCLUDE_DIRS} 124 | ${PCL_INCLUDE_DIRS} 125 | ) 126 | 127 | ## Declare a C++ library 128 | # add_library(${PROJECT_NAME} 129 | # src/${PROJECT_NAME}/ce30c_driver.cpp 130 | # ) 131 | 132 | ## Add cmake target dependencies of the library 133 | ## as an example, code may need to be generated before libraries 134 | ## either from message generation or dynamic reconfigure 135 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Declare a C++ executable 138 | ## With catkin_make all packages are built within a single CMake context 139 | ## The recommended prefix ensures that target names across packages don't collide 140 | # add_executable(${PROJECT_NAME}_node src/ce30c_driver_node.cpp) 141 | 142 | ## Rename C++ executable without prefix 143 | ## The above recommended prefix causes long target names, the following renames the 144 | ## target back to the shorter version for ease of user use 145 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 146 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 147 | 148 | ## Add cmake target dependencies of the executable 149 | ## same as for the library above 150 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 151 | 152 | ## Specify libraries to link a library or executable target against 153 | # target_link_libraries(${PROJECT_NAME}_node 154 | # ${catkin_LIBRARIES} 155 | # ) 156 | link_directories("${PROJECT_SOURCE_DIR}") 157 | add_executable(ce30_cloud src/ce30_cloud.cpp) 158 | 159 | target_link_libraries(ce30_cloud ${catkin_LIBRARIES} ${PCL_LIBRARIES} libbw_ce30v2.0.so) 160 | ############# 161 | ## Install ## 162 | ############# 163 | 164 | # all install targets should use catkin DESTINATION variables 165 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 166 | 167 | ## Mark executable scripts (Python etc.) for installation 168 | ## in contrast to setup.py, you can choose the destination 169 | # install(PROGRAMS 170 | # scripts/my_python_script 171 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark executables and/or libraries for installation 175 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 176 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark cpp header files for installation 182 | # install(DIRECTORY include/${PROJECT_NAME}/ 183 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 184 | # FILES_MATCHING PATTERN "*.h" 185 | # PATTERN ".svn" EXCLUDE 186 | # ) 187 | 188 | ## Mark other files for installation (e.g. launch and bag files, etc.) 189 | # install(FILES 190 | # # myfile1 191 | # # myfile2 192 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 193 | # ) 194 | 195 | ############# 196 | ## Testing ## 197 | ############# 198 | 199 | ## Add gtest based cpp test target and link libraries 200 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_ce30c_driver.cpp) 201 | # if(TARGET ${PROJECT_NAME}-test) 202 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 203 | # endif() 204 | 205 | ## Add folders to be run by python nosetests 206 | # catkin_add_nosetests(test) 207 | -------------------------------------------------------------------------------- /benewake_ce30c/README: -------------------------------------------------------------------------------- 1 | /************************************************************************************************************************************** 2 | /* Benewake Imaging LiDAR CE30-C ROS Package 3 | /* Version 2.0 4 | /************************************************************************************************************************************** 5 | 6 | /************************************************************************************************************************************** 7 | /* Environment 8 | /************************************************************************************************************************************** 9 | System: Ubuntu 16.04 10 | ROS: Kinetic 11 | 12 | On other platform, the package, as well as libbw_ce30vX.X.so file inside the package, should be re-compiled. The shared object file 13 | is generated by compiling CE30 linux SDK. 14 | 15 | /************************************************************************************************************************************** 16 | /* Run 17 | /************************************************************************************************************************************** 18 | $rosrun ce30c_driver ce30_cloud 19 | 20 | OR 21 | 22 | $roslaunch ce30c_driver benewake_ce30c.launch 23 | -------------------------------------------------------------------------------- /benewake_ce30c/include/ce30c_driver/base.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | //#define DEBUG 15 | #ifdef DEBUG 16 | #include 17 | #endif // DEBUG 18 | 19 | #define WIDTH 320 20 | #define HEIGHT 24 21 | #define TOTALDATA 2 * WIDTH * HEIGHT 22 | //#define DEBUG 23 | //#define PRINTDATA 24 | 25 | // define command buff 26 | const char kStart[51] = "getDistanceAndAmplitudeSorted "; // start command 27 | const char kStop[51] = "join "; // stop command 28 | const char kDisconnect[51] = "disconnect "; // disconnect command 29 | const char kGreyImage[51] = "enableFeatures 131072 "; // enable grey image 30 | const char kSetIntegTime[51]= "setIntegrationTime3D 1600 "; // integration time 1600 31 | const char kSetROI[51] = "roi 0 0 3 "; 32 | 33 | 34 | // Ethernet connect 35 | int TCP_connect(int &_sd, char *_ip){ 36 | int port = 50660; 37 | 38 | // CE_TCP 39 | _sd = socket(AF_INET, SOCK_STREAM, 0); 40 | if (_sd < 0){ 41 | // printf("Socket error!\n"); 42 | return -1; 43 | } 44 | struct sockaddr_in ser_addr = {0}; 45 | ser_addr.sin_family = AF_INET; 46 | ser_addr.sin_port = htons(port); 47 | const char* c_addr = _ip; 48 | ser_addr.sin_addr.s_addr = inet_addr(c_addr); 49 | 50 | // start connetion 51 | int ret = 0, total = 0; 52 | unsigned long ul = 1; 53 | ret = ioctl(_sd, FIONBIO, (unsigned long*)&ul); 54 | if(ret == -1){ 55 | close(_sd); 56 | return 0; 57 | } 58 | // printf("Device connecting ...\n"); 59 | ret = connect(_sd, (struct sockaddr*)&ser_addr, sizeof(ser_addr)); 60 | if(ret && errno != EINPROGRESS){ 61 | return 0; 62 | } 63 | if(ret == 0){ 64 | // printf("Connected!\n"); 65 | } 66 | else{ 67 | struct timeval tv; 68 | fd_set w; 69 | FD_ZERO(&w); 70 | FD_SET(_sd, &w); 71 | tv.tv_sec = 7; 72 | tv.tv_usec = 0; 73 | int retval = select(_sd+1, 0, &w, 0, &tv); 74 | if(retval == -1){ 75 | // select error 76 | return -1; 77 | } 78 | else if(retval == 0){ 79 | // connect timeout 80 | return -2; 81 | } 82 | else{ 83 | int er; 84 | socklen_t len = sizeof(er); 85 | if(getsockopt(_sd, SOL_SOCKET, SO_ERROR, (char*)&er, &len) < 0){ 86 | // getsocketopt error 87 | return -3; 88 | } 89 | if(er != 0){ 90 | // connnect error 91 | return -4; 92 | } 93 | } 94 | } 95 | ul = 0; 96 | ret = ioctl(_sd, FIONBIO, (unsigned long*)&ul); 97 | if(ret == -1){ 98 | // ioctl block error 99 | close(_sd); 100 | return -5; 101 | } 102 | struct timeval timeout = {3, 0}; 103 | setsockopt(_sd, SOL_SOCKET, SO_RCVTIMEO, (char*)&timeout, sizeof(timeout)); 104 | setsockopt(_sd, SOL_SOCKET, SO_SNDTIMEO, (char*)&timeout, sizeof(timeout)); 105 | 106 | return 1; 107 | } 108 | 109 | // receive data 110 | int recv_data(int _sd, unsigned char *_buff, int _length){ 111 | int total = 0, ret = 0; 112 | while(total != _length){ 113 | ret = recv(_sd, _buff + total, (_length - total), 0); 114 | if(ret < 0){ 115 | // printf("Data receiving failed!\n"); 116 | return -1; 117 | } 118 | #ifdef PRINTDATA 119 | else{ 120 | // printf("Receiving bytes: %d\n", ret); 121 | } 122 | #endif // PRINTDATA 123 | total += ret; 124 | } 125 | 126 | #ifdef PRINTDATA 127 | printf("Data: %02x, %02x, %02x, %02x, ...\n", _buff[0], _buff[1], _buff[2], _buff[3]); 128 | #endif // PRINTDATA 129 | return total; 130 | } 131 | 132 | // send command 133 | int send_command(int _sd, const char *_command, int _length){ 134 | int total = 0, ret = 0; 135 | do 136 | { 137 | ret = send(_sd, _command + total, (_length - total), 0); 138 | if(ret < 0) 139 | return false; 140 | total += ret; 141 | }while(total != _length); 142 | return total; 143 | } 144 | 145 | // translate char* to int 146 | int char_to_int(char *_c) 147 | { 148 | int value = 0; 149 | int len = strlen(_c); 150 | if(len < 0) 151 | return 0xffffffff; 152 | 153 | for(int i = len - 1; i >= 0; i --) 154 | { 155 | if(i == 0 && _c[0] == '-') 156 | { 157 | value *= -1; 158 | } 159 | else if(_c[i] >= '0' && _c[i] <= '9') 160 | { 161 | value += (int)(_c[i] - '0') * pow(10.0, (len - 1 - i)); 162 | } 163 | else 164 | { 165 | return 0xffffffff; 166 | } 167 | } 168 | 169 | return value; 170 | } 171 | 172 | // clear socket buffer 173 | void clearSocketBuffer(int _fd) 174 | { 175 | struct timeval tmOut; 176 | tmOut.tv_sec = 0; 177 | tmOut.tv_usec = 200000; 178 | fd_set fds; 179 | FD_ZERO(&fds); 180 | FD_SET(_fd, &fds); 181 | int nRet; 182 | char tmp[2]; 183 | memset(tmp, 0, sizeof(tmp)); 184 | while(1) 185 | { 186 | nRet = select(FD_SETSIZE, &fds, NULL, NULL, &tmOut); 187 | if(nRet == 0) 188 | break; 189 | recv(_fd, tmp, 1, 0); 190 | } 191 | } 192 | 193 | -------------------------------------------------------------------------------- /benewake_ce30c/include/ce30c_driver/ce30_sdk.h: -------------------------------------------------------------------------------- 1 | #include "undistort.h" 2 | extern "C" 3 | { 4 | namespace benewake 5 | { 6 | double gCameraMatrix[9], gDistCoeffs[4]; 7 | unsigned short gRawDistMatrix[HEIGHT * WIDTH] = {0}, gRawAmpMatrix[HEIGHT * WIDTH] = {0}; 8 | 9 | // @brief Establish Ethernet connection and initiallize LiDAR. 10 | // @param _sd Socket handle. 11 | // @param _ip IP address of LiDAR. 12 | bool initDevice(int &_sd, char *_ip); 13 | 14 | // @brief Start several times measurement 15 | // @param _sd Socket handle. 16 | // @param _times The times of measurement. When set to 0, the measurement will be continuous 17 | // untill the LiDAR receive stop measurement command. Otherwise, the measurement will 18 | // automatically stop when the times of measurement is achieved. 19 | bool startMeasurement(int _sd, int _times); 20 | 21 | // @brief Get distance and signal strength of LiDAR's FoV. 22 | // @param _sd Socket handle. 23 | // @param _dist Distance data buffer, the size should be 24[height] * 660[width]. 24 | // @param _amp Distance data buffer, the size should be 24[height] * 660[width]. 25 | // The data sequence is from field of view's left to right and then from top to bottom. 26 | bool getDistanceData(int _sd, unsigned short *_dist, unsigned short *_amp); 27 | 28 | // @brief Stop measurement. 29 | // @param _sd Socket handle. 30 | bool stopMeasurement(int _sd); 31 | 32 | // @brief Stop TCP communication. 33 | // @param _sd Socket handle. 34 | bool closeDevice(int &_sd); 35 | 36 | // @brief Transform distance data into point cloud . 37 | // @param _depth Distance data buffer. 38 | // @param _coordX x-axis coordinates of point cloud, the direction is from left to right. 39 | // @param _coordY y-axis coordinates of point cloud, the direction is from bottom to top. 40 | // @param _coordZ z-axis coordinates of point cloud, the direction is from near to far. 41 | // All params should have the same size of 24[height] * 660[width], and the same sequence 42 | // number params construct a space point (_coordX[n], _coordY[n], _coordZ[n]). 43 | // The data sequence is from field of view's left to right and then from top to bottom. 44 | bool getPointCloud(unsigned short *_dist, float *_coordX, float *_coordY, float *_coordZ); 45 | 46 | // @brief Change LiDAR's IP Address. 47 | // @param _sd Socket handle. 48 | // @param _newIP New IP address. 49 | bool changeIPAddress(int _sd, char *_newIP); 50 | } 51 | } 52 | -------------------------------------------------------------------------------- /benewake_ce30c/include/ce30c_driver/undistort.h: -------------------------------------------------------------------------------- 1 | #include "base.h" 2 | 3 | float bilinear_interpolation(float _targetX, float _targetY, float _P11, float _P12, float _P21, float _P22) 4 | { 5 | float alpha_x = _targetX - (int)_targetX; 6 | float alpha_y = _targetY - (int)_targetY; 7 | 8 | float interpl = _P11 * _P12 * _P21 * _P22; 9 | if(interpl != 0) 10 | { 11 | return ((1 - alpha_x) * (1 - alpha_y) * _P11 + alpha_x * (1 - alpha_y) * _P12 + (1 - alpha_x) * alpha_y * _P21 + alpha_x * alpha_y * _P22); 12 | } 13 | else 14 | { 15 | if(alpha_x < 0.5 && alpha_y < 0.5) 16 | return _P11; 17 | else if(alpha_x >= 0.5 && alpha_y < 0.5) 18 | return _P12; 19 | else if(alpha_x < 0.5 && alpha_y >= 0.5) 20 | return _P21; 21 | else 22 | return _P22; 23 | } 24 | } 25 | 26 | void remap(unsigned short *_src, unsigned short *_dst, float *_mapX, float *_mapY, int _h, int _w) 27 | { 28 | for (int i = 0; i < _h; i++) 29 | { 30 | for (int j = 0; j < _w; j++) 31 | { 32 | float targetX = _mapX[i * _w + j], targetY = _mapY[i * _w + j]; 33 | int x = (int)targetX, y = (int)targetY; 34 | if (y >= 0 && y <= HEIGHT - 2) 35 | { 36 | unsigned short p11 = _src[y * WIDTH + x]; 37 | unsigned short p12 = _src[y * WIDTH + (x + 1)]; 38 | unsigned short p21 = _src[(y + 1) * WIDTH + x]; 39 | unsigned short p22 = _src[(y + 1) * WIDTH + (x + 1)]; 40 | _dst[i * _w + j] = (unsigned short)bilinear_interpolation(targetX, targetY, p11, p12, p21, p22); 41 | } 42 | else 43 | { 44 | _dst[i * _w + j] = 0; 45 | } 46 | } 47 | } 48 | } 49 | 50 | // inverse matrix (simplified for camera matrix) 51 | void inverse_matrix(double *_A, double *_B, int _n) 52 | { 53 | int i, j, k; 54 | float mx, temp, iT; 55 | double *C = new double[_n * _n]; 56 | for (i = 0; i < _n; i++){ 57 | for (j = 0; j < _n; j++){ 58 | C[i * _n + j] = _A[i * _n + j]; 59 | _B[i * _n + j] = i == j ? 1.0 : 0.0; 60 | } 61 | } 62 | 63 | for (i = 0; i < _n; i++) 64 | { 65 | /*mx = C[i][i]; 66 | k = i; 67 | for (j = i + 1; j < _n; j++) 68 | { 69 | if (fabs(C[j][i]) > fabs(mx)) 70 | { 71 | mx = C[j][i]; 72 | k = j; 73 | } 74 | } 75 | 76 | if (k != i) 77 | { 78 | for (j = 0; j < _n; j++) 79 | { 80 | temp = C[i][j]; 81 | C[i][j] = C[k][j]; 82 | C[k][j] = temp; 83 | 84 | temp = _B[i][j]; 85 | _B[i][j] = _b[k][j]; 86 | _B[k][j] = temp; 87 | } 88 | }*/ 89 | 90 | temp = C[i * _n + i]; 91 | for (j = 0; j < _n; j++) 92 | { 93 | C[i * _n + j] = C[i * _n + j] / temp; 94 | _B[i * _n + j] = _B[i * _n + j] / temp; 95 | } 96 | 97 | for (j = 0; j < _n; j++) 98 | { 99 | if (j != i) 100 | { 101 | iT = C[j * _n + i]; 102 | for (k = 0; k < _n; k++) 103 | { 104 | C[j * _n + k] = C[j * _n + k] - C[i * _n + k] * iT; 105 | _B[j * _n + k] = _B[j * _n + k] - _B[i * _n + k] * iT; 106 | } 107 | } 108 | } 109 | } 110 | } 111 | 112 | void init_fisheye_map(double *_CameraMatrix, double *_Coeffs, float *_mapx, float *_mapy, int _height, int _width) 113 | { 114 | double *NewCameraMatrix = (double*)malloc(sizeof(double) * 3 * 3); 115 | memcpy(NewCameraMatrix, _CameraMatrix, 9 * sizeof(double)); 116 | NewCameraMatrix[2] = (_width - 1)* 0.5; 117 | NewCameraMatrix[5] = (_height - 1) * 0.5; 118 | 119 | double *ir = (double*)malloc(sizeof(double) * 3 * 3); 120 | inverse_matrix(NewCameraMatrix, ir, 3); 121 | double u0 = _CameraMatrix[2], v0 = _CameraMatrix[5]; 122 | double fx = _CameraMatrix[0], fy = _CameraMatrix[4]; 123 | 124 | for (int i = 0; i < _height; i++) 125 | { 126 | double _x = i * ir[1] + ir[2], _y = i * ir[4] + ir[5], _w = i * ir[7] + ir[8]; 127 | 128 | for (int j = 0; j < _width; j++, _x += ir[0], _y += ir[3], _w += ir[6]) 129 | { 130 | double x = _x/_w, y = _y/_w; 131 | 132 | double r = sqrt(x*x + y*y); 133 | double theta = atan(r); 134 | 135 | double theta2 = theta*theta, theta4 = theta2*theta2, theta6 = theta2*theta4, theta8 = theta4*theta4; 136 | double theta_d = theta * (1 + _Coeffs[0] * theta2 + _Coeffs[1] * theta4 + _Coeffs[2] * theta6 + _Coeffs[3] * theta8); 137 | 138 | double scale = (r == 0) ? 1.0 : theta_d / r; 139 | double u = fx * x * scale + u0; 140 | double v = fy * y * scale + v0; 141 | 142 | _mapx[i * _width + j] = (float)u; 143 | _mapy[i * _width + j] = (float)v; 144 | } 145 | } 146 | } 147 | -------------------------------------------------------------------------------- /benewake_ce30c/launch/benewake_ce30c.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /benewake_ce30c/launch/benewake_ce30c_change_ip.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /benewake_ce30c/launch/benewake_ce30c_new_address.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /benewake_ce30c/launch/benewake_ce30c_tif.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /benewake_ce30c/launch/hector_slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | transform_tolerance: 0.01 9 | min_height: 0.0 10 | max_height: 3.0 11 | angle_min: -0.418879 # 66 - 90deg 12 | angle_max: 2.72271 # +90 +66 deg 13 | angle_increment: 0.0087 # M_PI/360.0 14 | scan_time: 0.02 15 | range_min: 0.2 16 | range_max: 6.0 17 | use_inf: true 18 | concurrency_level: 1 19 | 20 | 21 | 22 | #-- when play a rosbag, use the option --clock 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 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /benewake_ce30c/launch/pointcloud_2_laserscan.launch: -------------------------------------------------------------------------------- 1 | 2 | # Note that the point cloud front is the Y axis 3 | 4 | 5 | 6 | 7 | transform_tolerance: 0.01 8 | min_height: 0.0 9 | max_height: 3.0 10 | angle_min: -0.418879 # 66 - 90deg 11 | angle_max: 2.72271 # +90 +66 deg 12 | angle_increment: 0.0087 13 | scan_time: 0.02 14 | range_min: 0.2 15 | range_max: 10.0 16 | use_inf: true 17 | concurrency_level: 1 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /benewake_ce30c/launch/test_tf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /benewake_ce30c/libbw_ce30v2.0.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/benewake_ce30c/libbw_ce30v2.0.so -------------------------------------------------------------------------------- /benewake_ce30c/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ce30c_driver 4 | 1.9.0 5 | The ce30c_driver package 6 | 7 | 8 | 9 | 10 | jxy 11 | tanwenquan 12 | 13 | 14 | 15 | 16 | 17 | TODO 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 | catkin 44 | roscpp 45 | rospy 46 | sensor_msgs 47 | std_msgs 48 | roscpp 49 | rospy 50 | sensor_msgs 51 | std_msgs 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /benewake_ce30c/readme: -------------------------------------------------------------------------------- 1 | installed pointcloud to laserscan package: 2 | sudo apt update 3 | sudo apt-get install ros-kinetic-pointcloud-to-laserscan 4 | 5 | play the rosbag using --clock argument to use sim time 6 | 7 | rosbag play ce30_02.bag --clock 8 | 9 | In the hector slam launch, use sime time parameter 10 | -------------------------------------------------------------------------------- /benewake_ce30c/rosbag/ce30_02.bag.7z: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/benewake_ce30c/rosbag/ce30_02.bag.7z -------------------------------------------------------------------------------- /benewake_ce30c/rviz/pointcloud.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 | Splitter Ratio: 0.5 10 | Tree Height: 775 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679016 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: PointCloud2 30 | Toolbars: 31 | toolButtonStyle: 2 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.314333349 57 | Min Value: -0.307433337 58 | Value: true 59 | Axis: Z 60 | Channel Name: intensity 61 | Class: rviz/PointCloud2 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: PointCloud2 72 | Position Transformer: XYZ 73 | Queue Size: 10 74 | Selectable: true 75 | Size (Pixels): 3 76 | Size (m): 0.00999999978 77 | Style: Flat Squares 78 | Topic: /ce30c_output 79 | Unreliable: false 80 | Use Fixed Frame: true 81 | Use rainbow: true 82 | Value: true 83 | - Alpha: 1 84 | Autocompute Intensity Bounds: true 85 | Autocompute Value Bounds: 86 | Max Value: 10 87 | Min Value: -10 88 | Value: true 89 | Axis: Z 90 | Channel Name: intensity 91 | Class: rviz/LaserScan 92 | Color: 255; 255; 255 93 | Color Transformer: Intensity 94 | Decay Time: 0 95 | Enabled: false 96 | Invert Rainbow: false 97 | Max Color: 255; 0; 127 98 | Max Intensity: 4096 99 | Min Color: 0; 0; 0 100 | Min Intensity: 0 101 | Name: LaserScan 102 | Position Transformer: XYZ 103 | Queue Size: 10 104 | Selectable: true 105 | Size (Pixels): 3 106 | Size (m): 0.0500000007 107 | Style: Flat Squares 108 | Topic: /scan 109 | Unreliable: false 110 | Use Fixed Frame: true 111 | Use rainbow: false 112 | Value: false 113 | - Class: rviz/Axes 114 | Enabled: true 115 | Length: 1 116 | Name: Axes 117 | Radius: 0.100000001 118 | Reference Frame: 119 | Value: true 120 | Enabled: true 121 | Global Options: 122 | Background Color: 48; 48; 48 123 | Default Light: true 124 | Fixed Frame: map 125 | Frame Rate: 30 126 | Name: root 127 | Tools: 128 | - Class: rviz/Interact 129 | Hide Inactive Objects: true 130 | - Class: rviz/MoveCamera 131 | - Class: rviz/Select 132 | - Class: rviz/FocusCamera 133 | - Class: rviz/Measure 134 | - Class: rviz/SetInitialPose 135 | Topic: /initialpose 136 | - Class: rviz/SetGoal 137 | Topic: /move_base_simple/goal 138 | - Class: rviz/PublishPoint 139 | Single click: true 140 | Topic: /clicked_point 141 | Value: true 142 | Views: 143 | Current: 144 | Class: rviz/Orbit 145 | Distance: 5.18725586 146 | Enable Stereo Rendering: 147 | Stereo Eye Separation: 0.0599999987 148 | Stereo Focal Distance: 1 149 | Swap Stereo Eyes: false 150 | Value: false 151 | Focal Point: 152 | X: 0 153 | Y: 0 154 | Z: 0 155 | Focal Shape Fixed Size: true 156 | Focal Shape Size: 0.0500000007 157 | Invert Z Axis: false 158 | Name: Current View 159 | Near Clip Distance: 0.00999999978 160 | Pitch: 0.815398753 161 | Target Frame: 162 | Value: Orbit (rviz) 163 | Yaw: 5.03354883 164 | Saved: ~ 165 | Window Geometry: 166 | Displays: 167 | collapsed: false 168 | Height: 1056 169 | Hide Left Dock: false 170 | Hide Right Dock: false 171 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 172 | Selection: 173 | collapsed: false 174 | Time: 175 | collapsed: false 176 | Tool Properties: 177 | collapsed: false 178 | Views: 179 | collapsed: false 180 | Width: 1855 181 | X: 65 182 | Y: 24 183 | -------------------------------------------------------------------------------- /benewake_ce30c/src/ce30_cloud.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #define PHEIGHT 24 8 | #define PWIDTH 660 9 | typedef struct weight{ 10 | float x; 11 | float y; 12 | float h; 13 | int n; 14 | }weight; 15 | typedef struct process{ 16 | std::vector p_s; 17 | std::vector p_e; 18 | std::vector p_x; 19 | std::vector p_h; 20 | int p_n; 21 | }process; 22 | class Cloud{ 23 | public: 24 | void init(); 25 | void camera(const float camera_f); 26 | void point_pose(const unsigned short *range); 27 | void pub_cloud(std::string _frame_id); 28 | std::vector diff_cloud(); 29 | std::vector clust_cloud(const unsigned short *range); 30 | process people_count(const std::vector w_p, process pre); 31 | 32 | pcl::PointCloud cloud; 33 | 34 | std::vector pose_x; 35 | std::vector pose_y; 36 | // std::vector pose_z; 37 | private: 38 | ros::NodeHandle nh; 39 | ros::Publisher pcl_pub; 40 | sensor_msgs::PointCloud2 output; 41 | std::vector env; 42 | }; 43 | 44 | void Cloud::init() 45 | { 46 | cloud.width = PWIDTH; 47 | cloud.height = PHEIGHT; 48 | cloud.points.resize(cloud.width * cloud.height); 49 | pcl_pub = nh.advertise ("ce30c_output", 1); 50 | Cloud::camera(150); 51 | } 52 | void Cloud::camera(const float camera_f) 53 | { 54 | pose_x.clear(); 55 | pose_y.clear(); 56 | // pose_z.clear(); 57 | for(int j = 0; j 10){ 75 | cloud.points[i].x = pose_x[i]*range[i]/100.0f; 76 | cloud.points[i].y = range[i]/100.0f; 77 | cloud.points[i].z = pose_y[i]*range[i]/100.0f; 78 | }else{ 79 | cloud.points[i].x = 0; 80 | cloud.points[i].y = 0; 81 | cloud.points[i].z = 0; 82 | } 83 | } 84 | } 85 | 86 | void Cloud::pub_cloud(std::string _frame_id) 87 | { 88 | pcl::toROSMsg(cloud, output); 89 | output.header.frame_id = _frame_id; 90 | output.header.stamp = ros::Time::now(); 91 | pcl_pub.publish(output); 92 | } 93 | 94 | bool gRun = true; 95 | 96 | void sig_stop(int value) 97 | { 98 | gRun = false; 99 | return; 100 | } 101 | 102 | int main(int argc, char **argv) 103 | { 104 | ros::init(argc, argv, "ce30_cloud"); 105 | ros::NodeHandle privnh("~"); 106 | gRun = true; 107 | signal(SIGINT, sig_stop); 108 | 109 | Cloud ce30_c; 110 | process result; 111 | result.p_n = 0; 112 | ce30_c.init(); 113 | printf("Init device\n"); 114 | int sd = 0; 115 | std::string ip, newip, frame_id; 116 | // get ip 117 | privnh.param("IP", ip, std::string("192.168.1.80")); 118 | privnh.param("frame_id", frame_id, std::string("ce30c_pointcloud")); 119 | 120 | int first = 0; 121 | // connect lidar 122 | char* addr = const_cast(ip.c_str()); 123 | ROS_INFO("Connect to: %s.", addr); 124 | if(!benewake::initDevice(sd, addr)) 125 | { 126 | ROS_ERROR("Connection failed! The node exited."); 127 | return -1; 128 | } 129 | ROS_INFO("Connection succeeded!"); 130 | // reset ip 131 | if(privnh.param("newIP", newip, std::string("192.168.1.80"))) 132 | { 133 | char* new_addr = const_cast(newip.c_str()); 134 | benewake::changeIPAddress(sd, new_addr); 135 | ROS_INFO("LiDAR is rebooting! The node exited. Please restart the node later with new IP address."); 136 | privnh.deleteParam("/ce30_cloud/newIP"); 137 | return 0; 138 | } 139 | 140 | int h = 24, w = 660; 141 | unsigned short data[h * w], amp[h * w] , tmp = 0; 142 | std::vector a; 143 | // start measurement 144 | if(!benewake::startMeasurement(sd, 0)) 145 | { 146 | ROS_ERROR("Start measurement failed! The node exited."); 147 | return -1; 148 | } 149 | ROS_INFO("Start measurement ..."); 150 | while(ros::ok()&&gRun) 151 | { 152 | if(!benewake::getDistanceData(sd, data, amp)) 153 | { 154 | benewake::stopMeasurement(sd); 155 | benewake::closeDevice(sd); 156 | close(sd); 157 | 158 | return -1; 159 | } 160 | else 161 | { 162 | ce30_c.point_pose(data); 163 | ce30_c.pub_cloud(frame_id); 164 | } 165 | } 166 | benewake::stopMeasurement(sd); 167 | ROS_INFO("Measurement stopped."); 168 | benewake::closeDevice(sd); 169 | ROS_INFO("Device Disconneted!"); 170 | close(sd); 171 | ROS_INFO("Socket closed!"); 172 | return 0; 173 | } 174 | -------------------------------------------------------------------------------- /benewake_ce30c/windows_software/CE30-C_PointCoud_Viewer_v1.0.0.7z: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/benewake_ce30c/windows_software/CE30-C_PointCoud_Viewer_v1.0.0.7z -------------------------------------------------------------------------------- /donkey_car/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(donkey_car) 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) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES donkey_car 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/donkey_car.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/donkey_car_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # install(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables and/or libraries for installation 164 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 165 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 166 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 167 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | # ) 169 | 170 | ## Mark cpp header files for installation 171 | # install(DIRECTORY include/${PROJECT_NAME}/ 172 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 173 | # FILES_MATCHING PATTERN "*.h" 174 | # PATTERN ".svn" EXCLUDE 175 | # ) 176 | 177 | ## Mark other files for installation (e.g. launch and bag files, etc.) 178 | # install(FILES 179 | # # myfile1 180 | # # myfile2 181 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 182 | # ) 183 | 184 | ############# 185 | ## Testing ## 186 | ############# 187 | 188 | ## Add gtest based cpp test target and link libraries 189 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_donkey_car.cpp) 190 | # if(TARGET ${PROJECT_NAME}-test) 191 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 192 | # endif() 193 | 194 | ## Add folders to be run by python nosetests 195 | # catkin_add_nosetests(test) 196 | -------------------------------------------------------------------------------- /donkey_car/launch/avoidance_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /donkey_car/launch/blob_chase_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /donkey_car/launch/keyboard_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /donkey_car/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | donkey_car 4 | 0.0.0 5 | The donkey_car package 6 | 7 | 8 | 9 | 10 | ubuntu 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 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /donkey_car/src/chase_the_ball.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | """ 3 | Gets the position of the blob and it commands to steer the wheels 4 | 5 | Subscribes to 6 | /blob/point_blob 7 | 8 | Publishes commands to 9 | /dkcar/control/cmd_vel 10 | 11 | """ 12 | import math, time 13 | import rospy 14 | from geometry_msgs.msg import Twist 15 | from geometry_msgs.msg import Point 16 | 17 | K_LAT_DIST_TO_STEER = 2.0 18 | 19 | def saturate(value, min, max): 20 | if value <= min: return(min) 21 | elif value >= max: return(max) 22 | else: return(value) 23 | 24 | class ChaseBall(): 25 | def __init__(self): 26 | 27 | self.blob_x = 0.0 28 | self.blob_y = 0.0 29 | self._time_detected = 0.0 30 | 31 | self.sub_center = rospy.Subscriber("/blob/point_blob", Point, self.update_ball) 32 | rospy.loginfo("Subscribers set") 33 | 34 | self.pub_twist = rospy.Publisher("/dkcar/control/cmd_vel", Twist, queue_size=5) 35 | rospy.loginfo("Publisher set") 36 | 37 | self._message = Twist() 38 | 39 | self._time_steer = 0 40 | self._steer_sign_prev = 0 41 | 42 | @property 43 | def is_detected(self): return(time.time() - self._time_detected < 1.0) 44 | 45 | def update_ball(self, message): 46 | self.blob_x = message.x 47 | self.blob_y = message.y 48 | self._time_detected = time.time() 49 | # rospy.loginfo("Ball detected: %.1f %.1f "%(self.blob_x, self.blob_y)) 50 | 51 | def get_control_action(self): 52 | """ 53 | Based on the current ranges, calculate the command 54 | 55 | Steer will be added to the commanded throttle 56 | throttle will be multiplied by the commanded throttle 57 | """ 58 | steer_action = 0.0 59 | throttle_action = 0.0 60 | 61 | if self.is_detected: 62 | #--- Apply steering, proportional to how close is the object 63 | steer_action =-K_LAT_DIST_TO_STEER*self.blob_x 64 | steer_action = saturate(steer_action, -1.5, 1.5) 65 | rospy.loginfo("Steering command %.2f"%steer_action) 66 | throttle_action = 1.0 67 | 68 | return (steer_action, throttle_action) 69 | 70 | def run(self): 71 | 72 | #--- Set the control rate 73 | rate = rospy.Rate(5) 74 | 75 | while not rospy.is_shutdown(): 76 | #-- Get the control action 77 | steer_action, throttle_action = self.get_control_action() 78 | 79 | rospy.loginfo("Steering = %3.1f"%(steer_action)) 80 | 81 | #-- update the message 82 | self._message.linear.x = throttle_action 83 | self._message.angular.z = steer_action 84 | 85 | #-- publish it 86 | self.pub_twist.publish(self._message) 87 | 88 | rate.sleep() 89 | 90 | if __name__ == "__main__": 91 | 92 | rospy.init_node('chase_ball') 93 | 94 | chase_ball = ChaseBall() 95 | chase_ball.run() -------------------------------------------------------------------------------- /donkey_car/src/low_level_control.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | """ 4 | Class for low level control of our car. It assumes ros-12cpwmboard has been 5 | installed 6 | """ 7 | import rospy 8 | from i2cpwm_board.msg import Servo, ServoArray 9 | from geometry_msgs.msg import Twist 10 | import time 11 | 12 | 13 | class ServoConvert(): 14 | def __init__(self, id=1, center_value=333, range=90, direction=1): 15 | self.value = 0.0 16 | self.value_out = center_value 17 | self._center = center_value 18 | self._range = range 19 | self._half_range= 0.5*range 20 | self._dir = direction 21 | self.id = id 22 | 23 | #--- Convert its range in [-1, 1] 24 | self._sf = 1.0/self._half_range 25 | 26 | def get_value_out(self, value_in): 27 | #--- value is in [-1, 1] 28 | self.value = value_in 29 | self.value_out = int(self._dir*value_in*self._half_range + self._center) 30 | print self.id, self.value_out 31 | return(self.value_out) 32 | 33 | class DkLowLevelCtrl(): 34 | def __init__(self): 35 | rospy.loginfo("Setting Up the Node...") 36 | 37 | rospy.init_node('dk_llc') 38 | 39 | self.actuators = {} 40 | self.actuators['throttle'] = ServoConvert(id=1) 41 | self.actuators['steering'] = ServoConvert(id=2, direction=1) #-- positive left 42 | rospy.loginfo("> Actuators corrrectly initialized") 43 | 44 | self._servo_msg = ServoArray() 45 | for i in range(2): self._servo_msg.servos.append(Servo()) 46 | 47 | #--- Create the servo array publisher 48 | self.ros_pub_servo_array = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1) 49 | rospy.loginfo("> Publisher corrrectly initialized") 50 | 51 | #--- Create the Subscriber to Twist commands 52 | self.ros_sub_twist = rospy.Subscriber("/cmd_vel", Twist, self.set_actuators_from_cmdvel) 53 | rospy.loginfo("> Subscriber corrrectly initialized") 54 | 55 | #--- Get the last time e got a commands 56 | self._last_time_cmd_rcv = time.time() 57 | self._timeout_s = 5 58 | 59 | rospy.loginfo("Initialization complete") 60 | 61 | def set_actuators_from_cmdvel(self, message): 62 | """ 63 | Get a message from cmd_vel, assuming a maximum input of 1 64 | """ 65 | #-- Save the time 66 | self._last_time_cmd_rcv = time.time() 67 | 68 | #-- Convert vel into servo values 69 | self.actuators['throttle'].get_value_out(message.linear.x) 70 | self.actuators['steering'].get_value_out(message.angular.z) 71 | rospy.loginfo("Got a command v = %2.1f s = %2.1f"%(message.linear.x, message.angular.z)) 72 | self.send_servo_msg() 73 | 74 | def set_actuators_idle(self): 75 | #-- Convert vel into servo values 76 | self.actuators['throttle'].get_value_out(0) 77 | self.actuators['steering'].get_value_out(0) 78 | rospy.loginfo("Setting actutors to idle") 79 | self.send_servo_msg() 80 | 81 | def send_servo_msg(self): 82 | for actuator_name, servo_obj in self.actuators.iteritems(): 83 | self._servo_msg.servos[servo_obj.id-1].servo = servo_obj.id 84 | self._servo_msg.servos[servo_obj.id-1].value = servo_obj.value_out 85 | rospy.loginfo("Sending to %s command %d"%(actuator_name, servo_obj.value_out)) 86 | 87 | self.ros_pub_servo_array.publish(self._servo_msg) 88 | 89 | @property 90 | def is_controller_connected(self): 91 | print time.time() - self._last_time_cmd_rcv 92 | return(time.time() - self._last_time_cmd_rcv < self._timeout_s) 93 | 94 | def run(self): 95 | 96 | #--- Set the control rate 97 | rate = rospy.Rate(10) 98 | 99 | while not rospy.is_shutdown(): 100 | print self._last_time_cmd_rcv, self.is_controller_connected 101 | if not self.is_controller_connected: 102 | self.set_actuators_idle() 103 | 104 | rate.sleep() 105 | 106 | if __name__ == "__main__": 107 | dk_llc = DkLowLevelCtrl() 108 | dk_llc.run() 109 | -------------------------------------------------------------------------------- /donkey_car/src/low_level_control_blob_chase.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | """ 4 | Class for low level control of our car. It assumes ros-12cpwmboard has been 5 | installed. 6 | 7 | Listens to /dkcar/control/cmd_vel for corrective actions to the /cmd_vel coming from keyboard or joystick 8 | 9 | """ 10 | import rospy 11 | from i2cpwm_board.msg import Servo, ServoArray 12 | from geometry_msgs.msg import Twist 13 | import time 14 | 15 | 16 | class ServoConvert(): 17 | def __init__(self, id=1, center_value=333, range=90, direction=1): 18 | self.value = 0.0 19 | self.value_out = center_value 20 | self._center = center_value 21 | self._range = range 22 | self._half_range= 0.5*range 23 | self._dir = direction 24 | self.id = id 25 | 26 | #--- Convert its range in [-1, 1] 27 | self._sf = 1.0/self._half_range 28 | 29 | def get_value_out(self, value_in): 30 | #--- value is in [-1, 1] 31 | self.value = value_in 32 | self.value_out = int(self._dir*value_in*self._half_range + self._center) 33 | # print self.id, self.value_out 34 | return(self.value_out) 35 | 36 | def saturate(value, min, max): 37 | if value <= min: return(min) 38 | elif value >= max: return(max) 39 | else: return(value) 40 | 41 | class DkLowLevelCtrl(): 42 | def __init__(self): 43 | rospy.loginfo("Setting Up the Node...") 44 | 45 | #--- Initialize the node 46 | rospy.init_node('dk_llc') 47 | 48 | self.actuators = {} 49 | self.actuators['throttle'] = ServoConvert(id=1) 50 | self.actuators['steering'] = ServoConvert(id=2, center_value=328, direction=1) #-- positive left 51 | rospy.loginfo("> Actuators corrrectly initialized") 52 | 53 | self._servo_msg = ServoArray() 54 | for i in range(2): self._servo_msg.servos.append(Servo()) 55 | 56 | #--- Create the servo array publisher 57 | self.ros_pub_servo_array = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1) 58 | rospy.loginfo("> Publisher corrrectly initialized") 59 | 60 | #--- Create a debug publisher for resulting cmd_vel 61 | self.ros_pub_debug_command = rospy.Publisher("/dkcar/debug/cmd_vel", Twist, queue_size=1) 62 | rospy.loginfo("> Publisher corrrectly initialized") 63 | 64 | #--- Create the Subscriber to Twist commands 65 | self.ros_sub_twist = rospy.Subscriber("/cmd_vel", Twist, self.update_message_from_command) 66 | rospy.loginfo("> Subscriber corrrectly initialized") 67 | 68 | #--- Create the Subscriber to obstacle_avoidance commands 69 | self.ros_sub_twist = rospy.Subscriber("/dkcar/control/cmd_vel", Twist, self.update_message_from_chase) 70 | rospy.loginfo("> Subscriber corrrectly initialized") 71 | 72 | self.throttle_cmd = 0. 73 | self.throttle_chase = 1. 74 | self.steer_cmd = 0. 75 | self.steer_chase = 0. 76 | 77 | self._debud_command_msg = Twist() 78 | 79 | #--- Get the last time e got a commands 80 | self._last_time_cmd_rcv = time.time() 81 | self._last_time_chase_rcv = time.time() 82 | self._timeout_ctrl = 100 83 | self._timeout_blob = 1 84 | 85 | rospy.loginfo("Initialization complete") 86 | 87 | def update_message_from_command(self, message): 88 | self._last_time_cmd_rcv = time.time() 89 | self.throttle_cmd = message.linear.x 90 | self.steer_cmd = message.angular.z 91 | 92 | def update_message_from_chase(self, message): 93 | self._last_time_chase_rcv = time.time() 94 | self.throttle_chase = message.linear.x 95 | self.steer_chase = message.angular.z 96 | print self.throttle_chase, self.steer_chase 97 | 98 | def compose_command_velocity(self): 99 | self.throttle = saturate(self.throttle_cmd*self.throttle_chase, -1, 1) 100 | 101 | #-- Add steering 102 | self.steer = saturate(self.steer_cmd + self.steer_chase, -1, 1) 103 | 104 | self._debud_command_msg.linear.x = self.throttle 105 | self._debud_command_msg.angular.z = self.steer 106 | 107 | self.ros_pub_debug_command.publish(self._debud_command_msg) 108 | 109 | self.set_actuators_from_cmdvel(self.throttle, self.steer) 110 | 111 | def set_actuators_from_cmdvel(self, throttle, steering): 112 | """ 113 | Get a message from cmd_vel, assuming a maximum input of 1 114 | """ 115 | #-- Convert vel into servo values 116 | self.actuators['throttle'].get_value_out(throttle) 117 | self.actuators['steering'].get_value_out(steering) 118 | # rospy.loginfo("Got a command v = %2.1f s = %2.1f"%(throttle, steering)) 119 | self.send_servo_msg() 120 | 121 | def set_actuators_idle(self): 122 | #-- Convert vel into servo values 123 | self.throttle_cmd = 0. 124 | self.steer_cmd = 0. 125 | 126 | def reset_avoid(self): 127 | self.throttle_chase = 1. 128 | self.steer_avoid = 0. 129 | 130 | def send_servo_msg(self): 131 | for actuator_name, servo_obj in self.actuators.iteritems(): 132 | self._servo_msg.servos[servo_obj.id-1].servo = servo_obj.id 133 | self._servo_msg.servos[servo_obj.id-1].value = servo_obj.value_out 134 | # rospy.loginfo("Sending to %s command %d"%(actuator_name, servo_obj.value_out)) 135 | 136 | self.ros_pub_servo_array.publish(self._servo_msg) 137 | 138 | @property 139 | def is_controller_connected(self): 140 | # print time.time() - self._last_time_cmd_rcv 141 | return(time.time() - self._last_time_cmd_rcv < self._timeout_ctrl) 142 | 143 | @property 144 | def is_chase_connected(self): 145 | return(time.time() - self._last_time_chase_rcv < self._timeout_blob) 146 | 147 | def run(self): 148 | 149 | #--- Set the control rate 150 | rate = rospy.Rate(10) 151 | 152 | while not rospy.is_shutdown(): 153 | self.compose_command_velocity() 154 | 155 | # print self._last_time_cmd_rcv, self.is_controller_connected 156 | if not self.is_controller_connected: 157 | self.set_actuators_idle() 158 | 159 | if not self.is_chase_connected: 160 | self.reset_avoid() 161 | 162 | 163 | rate.sleep() 164 | 165 | if __name__ == "__main__": 166 | dk_llc = DkLowLevelCtrl() 167 | dk_llc.run() 168 | -------------------------------------------------------------------------------- /donkey_car/src/low_level_control_with_sonar.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | """ 4 | Class for low level control of our car. It assumes ros-12cpwmboard has been 5 | installed. 6 | 7 | Listens to /dkcar/control/cmd_vel for corrective actions and avoid obstacles 8 | """ 9 | import rospy 10 | from i2cpwm_board.msg import Servo, ServoArray 11 | from geometry_msgs.msg import Twist 12 | import time 13 | 14 | 15 | class ServoConvert(): 16 | def __init__(self, id=1, center_value=333, range=90, direction=1): 17 | self.value = 0.0 18 | self.value_out = center_value 19 | self._center = center_value 20 | self._range = range 21 | self._half_range= 0.5*range 22 | self._dir = direction 23 | self.id = id 24 | 25 | #--- Convert its range in [-1, 1] 26 | self._sf = 1.0/self._half_range 27 | 28 | def get_value_out(self, value_in): 29 | #--- value is in [-1, 1] 30 | self.value = value_in 31 | self.value_out = int(self._dir*value_in*self._half_range + self._center) 32 | # print self.id, self.value_out 33 | return(self.value_out) 34 | 35 | def saturate(value, min, max): 36 | if value <= min: return(min) 37 | elif value >= max: return(max) 38 | else: return(value) 39 | 40 | class DkLowLevelCtrl(): 41 | def __init__(self): 42 | rospy.loginfo("Setting Up the Node...") 43 | 44 | #--- Initialize the node 45 | rospy.init_node('dk_llc') 46 | 47 | self.actuators = {} 48 | self.actuators['throttle'] = ServoConvert(id=1) 49 | self.actuators['steering'] = ServoConvert(id=2, center_value=328, direction=1) #-- positive left 50 | rospy.loginfo("> Actuators corrrectly initialized") 51 | 52 | self._servo_msg = ServoArray() 53 | for i in range(2): self._servo_msg.servos.append(Servo()) 54 | 55 | #--- Create the servo array publisher 56 | self.ros_pub_servo_array = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1) 57 | rospy.loginfo("> Publisher corrrectly initialized") 58 | 59 | #--- Create a debug publisher for resulting cmd_vel 60 | self.ros_pub_debug_command = rospy.Publisher("/dkcar/debug/cmd_vel", Twist, queue_size=1) 61 | rospy.loginfo("> Publisher corrrectly initialized") 62 | 63 | #--- Create the Subscriber to Twist commands 64 | self.ros_sub_twist = rospy.Subscriber("/cmd_vel", Twist, self.update_message_from_command) 65 | rospy.loginfo("> Subscriber corrrectly initialized") 66 | 67 | #--- Create the Subscriber to obstacle_avoidance commands 68 | self.ros_sub_twist = rospy.Subscriber("/dkcar/control/cmd_vel", Twist, self.update_message_from_avoid) 69 | rospy.loginfo("> Subscriber corrrectly initialized") 70 | 71 | self.throttle_cmd = 0. 72 | self.throttle_avoid = 1. 73 | self.steer_cmd = 0. 74 | self.steer_avoid = 0. 75 | 76 | self._debud_command_msg = Twist() 77 | 78 | #--- Get the last time e got a commands 79 | self._last_time_cmd_rcv = time.time() 80 | self._last_time_avoid_rcv = time.time() 81 | self._timeout_s = 5 82 | 83 | rospy.loginfo("Initialization complete") 84 | 85 | def update_message_from_command(self, message): 86 | self._last_time_cmd_rcv = time.time() 87 | self.throttle_cmd = message.linear.x 88 | self.steer_cmd = message.angular.z 89 | 90 | def update_message_from_avoid(self, message): 91 | self._last_time_avoid_rcv = time.time() 92 | self.throttle_avoid = message.linear.x 93 | self.steer_avoid = message.angular.z 94 | 95 | def compose_command_velocity(self): 96 | self.throttle = saturate(self.throttle_cmd*self.throttle_avoid, -1, 1) 97 | 98 | #-- Add steering only if positive throttle 99 | if self.throttle_cmd > 0: 100 | self.steer = saturate(self.steer_cmd + self.steer_avoid, -1, 1) 101 | else: 102 | self.steer = self.steer_cmd 103 | 104 | self._debud_command_msg.linear.x = self.throttle 105 | self._debud_command_msg.angular.z = self.steer 106 | 107 | self.ros_pub_debug_command.publish(self._debud_command_msg) 108 | 109 | self.set_actuators_from_cmdvel(self.throttle, self.steer) 110 | 111 | def set_actuators_from_cmdvel(self, throttle, steering): 112 | """ 113 | Get a message from cmd_vel, assuming a maximum input of 1 114 | """ 115 | #-- Convert vel into servo values 116 | self.actuators['throttle'].get_value_out(throttle) 117 | self.actuators['steering'].get_value_out(steering) 118 | # rospy.loginfo("Got a command v = %2.1f s = %2.1f"%(throttle, steering)) 119 | self.send_servo_msg() 120 | 121 | def set_actuators_idle(self): 122 | #-- Convert vel into servo values 123 | self.throttle_cmd = 0. 124 | self.steer_cmd = 0. 125 | 126 | def reset_avoid(self): 127 | self.throttle_avoid = 1. 128 | self.steer_avoid = 0. 129 | 130 | def send_servo_msg(self): 131 | for actuator_name, servo_obj in self.actuators.iteritems(): 132 | self._servo_msg.servos[servo_obj.id-1].servo = servo_obj.id 133 | self._servo_msg.servos[servo_obj.id-1].value = servo_obj.value_out 134 | # rospy.loginfo("Sending to %s command %d"%(actuator_name, servo_obj.value_out)) 135 | 136 | self.ros_pub_servo_array.publish(self._servo_msg) 137 | 138 | @property 139 | def is_controller_connected(self): 140 | # print time.time() - self._last_time_cmd_rcv 141 | return(time.time() - self._last_time_cmd_rcv < self._timeout_s) 142 | 143 | @property 144 | def is_avoid_connected(self): 145 | # print time.time() - self._last_time_avoid_rcv 146 | return(time.time() - self._last_time_avoid_rcv < self._timeout_s) 147 | 148 | def run(self): 149 | 150 | #--- Set the control rate 151 | rate = rospy.Rate(10) 152 | 153 | while not rospy.is_shutdown(): 154 | self.compose_command_velocity() 155 | 156 | # print self._last_time_cmd_rcv, self.is_controller_connected 157 | if not self.is_controller_connected: 158 | self.set_actuators_idle() 159 | 160 | if not self.is_avoid_connected: 161 | self.reset_avoid() 162 | 163 | 164 | rate.sleep() 165 | 166 | if __name__ == "__main__": 167 | dk_llc = DkLowLevelCtrl() 168 | dk_llc.run() 169 | -------------------------------------------------------------------------------- /donkey_car/src/obstacle_avoid_sonar.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | """ 3 | Sonar array composed of 3 sonars (left, center, right) 4 | 5 | Subscribes to the three topics 0 (left) 1(center) 2 (right) 6 | 7 | calculates a correction to the cmd_vel that is: 8 | multiplicative for x 9 | additive for angle 10 | 11 | """ 12 | import math, time 13 | import rospy 14 | from sensor_msgs.msg import Range 15 | from geometry_msgs.msg import Twist 16 | 17 | DIST_STEER_ENGAGE = 1.2 18 | DIST_BREAK = 0.4 19 | 20 | DIST_LAT_ENGAGE = 0.4 21 | 22 | K_FRONT_DIST_TO_SPEED = 1.0 23 | K_LAT_DIST_TO_STEER = 2.0 24 | 25 | TIME_KEEP_STEERING = 1.5 26 | 27 | def saturate(value, min, max): 28 | if value <= min: return(min) 29 | elif value >= max: return(max) 30 | else: return(value) 31 | 32 | class ObstAvoid(): 33 | def __init__(self): 34 | 35 | self.range_center = 3 36 | self.range_left = 3 37 | self.range_right = 3 38 | 39 | self.sub_center = rospy.Subscriber("/dkcar/sonar/1", Range, self.update_range) 40 | self.sub_left = rospy.Subscriber("/dkcar/sonar/0", Range, self.update_range) 41 | self.sub_right = rospy.Subscriber("/dkcar/sonar/2", Range, self.update_range) 42 | rospy.loginfo("Subscribers set") 43 | 44 | self.pub_twist = rospy.Publisher("/dkcar/control/cmd_vel", Twist, queue_size=5) 45 | rospy.loginfo("Publisher set") 46 | 47 | self._message = Twist() 48 | 49 | self._time_steer = 0 50 | self._steer_sign_prev = 0 51 | 52 | def update_range(self, message): 53 | angle = message.field_of_view 54 | 55 | if abs(angle) < 0.1: 56 | self.range_center = message.range 57 | 58 | elif angle > 0: 59 | self.range_right = message.range 60 | 61 | elif angle < 0: 62 | self.range_left = message.range 63 | 64 | # rospy.loginfo("Sonar array: %.1f %.1f %.1f"%(self.range_left, self.range_center, self.range_right)) 65 | 66 | def get_control_action(self): 67 | """ 68 | Based on the current ranges, calculate the command 69 | """ 70 | 71 | break_action = 1.0 72 | steer_action = 0.0 73 | 74 | #--- Get the minimum distance 75 | range = min([self.range_center, self.range_left, self.range_right]) 76 | # print "%.2f %.2f %.2f %.2f"%(range, self.range_left, self.range_center, self.range_right) 77 | 78 | if self.range_center < DIST_STEER_ENGAGE: 79 | #--- Start applying the break 80 | # break_action = (range - DIST_BREAK)/(DIST_STEER_ENGAGE - DIST_BREAK) 81 | adim_dist = range/DIST_STEER_ENGAGE 82 | if range < DIST_BREAK: 83 | break_action = K_FRONT_DIST_TO_SPEED*(range/DIST_BREAK) 84 | break_action = saturate(break_action, 0, 1) 85 | rospy.loginfo("Engaging break %.1f"%break_action) 86 | 87 | #--- Apply steering, proportional to how close is the object 88 | steer_action = K_LAT_DIST_TO_STEER*(1.0 - adim_dist) 89 | steer_action = self.get_signed_steer(steer_action) 90 | 91 | steer_action = saturate(steer_action, -1.5, 1.5) 92 | rospy.loginfo("Steering command %.2f"%steer_action) 93 | 94 | return (break_action, steer_action) 95 | 96 | def get_signed_steer(self, steer_action): 97 | 98 | if time.time() > self._time_steer + TIME_KEEP_STEERING: 99 | print ("> Update steer_action sign") 100 | self._time_steer = time.time() 101 | 102 | #-- If the lwft object is closer, turn right (negative) 103 | if self.range_left < self.range_right: 104 | steer_action = -steer_action 105 | 106 | if steer_action >= 0: self._steer_sign_prev = 1 107 | else: self._steer_sign_prev = -1 108 | 109 | else: 110 | steer_action *= self._steer_sign_prev 111 | 112 | return (steer_action) 113 | 114 | def run(self): 115 | 116 | #--- Set the control rate 117 | rate = rospy.Rate(5) 118 | 119 | while not rospy.is_shutdown(): 120 | #-- Get the control action 121 | break_action, steer_action = self.get_control_action() 122 | 123 | # rospy.loginfo("Throttle = %3.1f Steering = %3.1f"%(break_action, steer_action)) 124 | 125 | #-- update the message 126 | self._message.linear.x = break_action 127 | self._message.angular.z = steer_action 128 | 129 | #-- publish it 130 | self.pub_twist.publish(self._message) 131 | 132 | rate.sleep() 133 | 134 | if __name__ == "__main__": 135 | 136 | rospy.init_node('obstacle_avoid') 137 | 138 | obst_avoid = ObstAvoid() 139 | obst_avoid.run() -------------------------------------------------------------------------------- /donkey_sonar/3dprint/TfMini Bracket.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/donkey_sonar/3dprint/TfMini Bracket.STL -------------------------------------------------------------------------------- /donkey_sonar/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(donkey_sonar) 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 | rospy 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 exec_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 exec_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 exec_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 donkey_sonar 106 | # CATKIN_DEPENDS rospy 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}/donkey_sonar.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/donkey_sonar_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_donkey_sonar.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 | -------------------------------------------------------------------------------- /donkey_sonar/launch/sonar_array.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /donkey_sonar/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | donkey_sonar 4 | 0.0.0 5 | The donkey_sonar package 6 | 7 | 8 | 9 | 10 | ubuntu 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 | rospy 53 | rospy 54 | rospy 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /donkey_sonar/src/sonar.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | #-- FROM https://pimylifeup.com/raspberry-pi-distance-sensor/ 3 | import RPi.GPIO as GPIO 4 | import time 5 | 6 | class Sonar(): 7 | 8 | def __init__(self, gpio_trigger, gpio_echo, range_min=10, range_max=400): 9 | 10 | GPIO.setmode(GPIO.BCM) 11 | 12 | self._gpio_trigger = gpio_trigger 13 | self._gpio_echo = gpio_echo 14 | self._range_min = range_min 15 | self._range_max = range_max 16 | self._is_reading = False 17 | 18 | self._speed_sound = 17150.0 #- divided by 2 in cm/s 19 | 20 | self._last_time_reading = 0 21 | self._timeout = range_max/self._speed_sound*2 22 | 23 | GPIO.setup(gpio_trigger, GPIO.OUT) 24 | GPIO.setup(gpio_echo, GPIO.IN) 25 | 26 | #- Waiting for sensor to settle 27 | GPIO.output(gpio_trigger, GPIO.LOW) 28 | time.sleep(1) 29 | 30 | 31 | def get_range(self): 32 | self._is_reading = True 33 | #--- Call for a reading 34 | GPIO.output(self._gpio_trigger, GPIO.HIGH) 35 | time.sleep(0.00001) 36 | GPIO.output(self._gpio_trigger, GPIO.LOW) 37 | 38 | GPIO.output(self._gpio_trigger, GPIO.HIGH) 39 | time.sleep(0.00001) 40 | GPIO.output(self._gpio_trigger, GPIO.LOW) 41 | 42 | 43 | pulse_start_time = time.time() 44 | pulse_end_time = time.time() 45 | #--- Wait for the answer 46 | while GPIO.input(self._gpio_echo)==0: 47 | pulse_start_time = time.time() 48 | 49 | time0= time.time() 50 | while GPIO.input(self._gpio_echo)==1: 51 | pulse_end_time = time.time() 52 | # if time.time() - time0 > self._timeout: 53 | # self._is_reading = False 54 | # # print time.time() - time0 55 | # # print self._timeout 56 | # # print("TIMEOUT") 57 | # return (-1) 58 | 59 | self._last_time_reading = time.time() 60 | self._is_reading = False 61 | 62 | pulse_duration = pulse_end_time - pulse_start_time 63 | distance = pulse_duration * self._speed_sound 64 | 65 | if distance > self._range_max: 66 | distance = self._range_max 67 | 68 | if distance < self._range_min: 69 | distance = self._range_min 70 | # distance = -1 71 | 72 | return(distance) 73 | 74 | @property 75 | def is_reading(self): 76 | return(self._is_reading) 77 | 78 | if __name__ == "__main__": 79 | #-- FRONT 80 | PIN_TRIGGER = 5 81 | PIN_ECHO = 6 82 | 83 | #-- RIGHT 84 | # PIN_TRIGGER = 27 85 | # PIN_ECHO = 22 86 | 87 | #-- LEFT 88 | # PIN_TRIGGER = 4 89 | # PIN_ECHO = 17 90 | 91 | sonar = Sonar(PIN_TRIGGER, PIN_ECHO) 92 | 93 | while True: 94 | d = sonar.get_range() 95 | if d>0: print "Distance = %4.1f cm"%d -------------------------------------------------------------------------------- /donkey_sonar/src/sonar_array.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | """ 3 | Sonar array composed of 3 sonars (left, center, right) 4 | """ 5 | 6 | from sonar import Sonar 7 | import math 8 | import rospy 9 | from sensor_msgs.msg import Range 10 | 11 | """ 12 | sensor_msgs/Range 13 | 14 | uint8 ULTRASOUND=0 15 | uint8 INFRARED=1 16 | std_msgs/Header header 17 | uint32 seq 18 | time stamp 19 | string frame_id 20 | uint8 radiation_type 21 | float32 field_of_view 22 | float32 min_range 23 | float32 max_range 24 | float32 range 25 | 26 | """ 27 | 28 | #------- PARAMETERS 29 | NUM_SONAR = 3 30 | #-- CENTER 31 | SONAR_CENTER_GPIO_TRIGGER = 5 32 | SONAR_CENTER_GPIO_ECHO = 6 33 | 34 | #-- RIGHT 35 | SONAR_RIGHT_GPIO_TRIGGER = 27 36 | SONAR_RIGHT_GPIO_ECHO = 22 37 | 38 | #-- LEFT 39 | SONAR_LEFT_GPIO_TRIGGER = 4 40 | SONAR_LEFT_GPIO_ECHO = 17 41 | 42 | class SonarArray(): 43 | def __init__(self, 44 | num_sonar, 45 | gpio_trigger_list, #-- list of all the trigger pins, starting from left 46 | gpio_echo_list, #-- list of all the echo pins, starting from left 47 | range_min, #- [m] 48 | range_max, #- [m] 49 | angle_min_deg, #- [deg] 50 | angle_max_deg 51 | ): 52 | 53 | self.sonar_array = [] 54 | self.pub_array = [] 55 | self.num_sonar = num_sonar 56 | 57 | delta_angle_deg = (angle_max_deg-angle_min_deg)/float(num_sonar-1) 58 | 59 | rospy.loginfo("Initializing the arrays") 60 | #--- Create an array and expand the object with its angle 61 | for i in range(num_sonar): 62 | sonar = Sonar(gpio_trigger_list[i], gpio_echo_list[i], range_min=range_min*100, range_max=range_max*100) 63 | angle_deg = angle_min_deg + delta_angle_deg*i 64 | sonar.angle = math.radians(angle_deg) 65 | self.sonar_array.append(sonar) 66 | rospy.loginfo("Sonar %d set"%i) 67 | 68 | #--- Publishers 69 | topic_name = "/dkcar/sonar/%d"%i 70 | pub = rospy.Publisher(topic_name, Range, queue_size=5) 71 | self.pub_array.append(pub) 72 | rospy.loginfo("Publisher %d set with topic %s"%(i, topic_name)) 73 | 74 | #--- Default message 75 | message = Range() 76 | # message.ULTRASOUND = 1 77 | # message.INFRARED = 0 78 | message.radiation_type = 0 79 | message.min_range = range_min 80 | message.max_range = range_max 81 | self._message = message 82 | 83 | 84 | def scan(self): 85 | 86 | range_array = [] 87 | for i in range(self.num_sonar): 88 | range_cm = self.sonar_array[i].get_range() 89 | range_array.append(range_cm*0.01) 90 | self._message.range = range_cm*0.01 91 | self._message.field_of_view = self.sonar_array[i].angle #-- put the angle in field of view 92 | self.pub_array[i].publish(self._message) 93 | 94 | rospy.loginfo("Range [m]: left = %4.2f center = %4.2f right = %4.2f"%(range_array[0], 95 | range_array[1], range_array[2])) 96 | 97 | def run(self): 98 | #--- Set the control rate 99 | rate = rospy.Rate(10) 100 | 101 | rospy.loginfo("Running...") 102 | while not rospy.is_shutdown(): 103 | self.scan() 104 | rate.sleep() 105 | 106 | rospy.loginfo("Stopped") 107 | 108 | if __name__ == "__main__": 109 | 110 | rospy.loginfo("Setting Up the Sonar Node...") 111 | 112 | rospy.init_node('sonar_array') 113 | 114 | sonar_array = SonarArray(num_sonar = NUM_SONAR, 115 | gpio_trigger_list = [SONAR_LEFT_GPIO_TRIGGER, SONAR_CENTER_GPIO_TRIGGER, SONAR_RIGHT_GPIO_TRIGGER], 116 | gpio_echo_list = [SONAR_LEFT_GPIO_ECHO, SONAR_CENTER_GPIO_ECHO, SONAR_RIGHT_GPIO_ECHO], 117 | range_min = 0.2, 118 | range_max = 3.5, 119 | angle_min_deg = -30, 120 | angle_max_deg = 30 121 | ) 122 | sonar_array.run() 123 | -------------------------------------------------------------------------------- /donkey_sonar/test/test_sonar.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | #-- FROM https://pimylifeup.com/raspberry-pi-distance-sensor/ 3 | import RPi.GPIO as GPIO 4 | import time 5 | 6 | try: 7 | GPIO.setmode(GPIO.BCM) 8 | 9 | #-- FRONT 10 | PIN_TRIGGER = 5 11 | PIN_ECHO = 6 12 | 13 | #-- RIGHT 14 | # PIN_TRIGGER = 27 15 | # PIN_ECHO = 22 16 | 17 | #-- LEFT 18 | # PIN_TRIGGER = 4 19 | # PIN_ECHO = 17 20 | 21 | GPIO.setup(PIN_TRIGGER, GPIO.OUT) 22 | GPIO.setup(PIN_ECHO, GPIO.IN) 23 | 24 | GPIO.output(PIN_TRIGGER, GPIO.LOW) 25 | 26 | print "Waiting for sensor to settle" 27 | 28 | time.sleep(2) 29 | 30 | print "Calculating distance" 31 | 32 | while True: 33 | GPIO.output(PIN_TRIGGER, GPIO.HIGH) 34 | 35 | time.sleep(0.00001) 36 | 37 | GPIO.output(PIN_TRIGGER, GPIO.LOW) 38 | 39 | while GPIO.input(PIN_ECHO)==0: 40 | pulse_start_time = time.time() 41 | while GPIO.input(PIN_ECHO)==1: 42 | pulse_end_time = time.time() 43 | 44 | pulse_duration = pulse_end_time - pulse_start_time 45 | distance = round(pulse_duration * 17150, 2) 46 | print "Distance:",distance,"cm" 47 | 48 | finally: 49 | GPIO.cleanup() -------------------------------------------------------------------------------- /laser_scanner_tfmini/3dprint/TfMini Bracket.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/laser_scanner_tfmini/3dprint/TfMini Bracket.STL -------------------------------------------------------------------------------- /laser_scanner_tfmini/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(laser_scanner_tfmini) 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 | rospy 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 exec_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 exec_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 exec_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 laser_scanner_tfmini 106 | # CATKIN_DEPENDS rospy 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}/laser_scanner_tfmini.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/laser_scanner_tfmini_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_laser_scanner_tfmini.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 | -------------------------------------------------------------------------------- /laser_scanner_tfmini/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | laser_scanner_tfmini 4 | 0.0.0 5 | The laser_scanner_tfmini package 6 | 7 | 8 | 9 | 10 | ubuntu 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 | rospy 53 | rospy 54 | rospy 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /laser_scanner_tfmini/src/readme.txt: -------------------------------------------------------------------------------- 1 | servoblaster.py: 2 | 3 | library for handling servoblaster library (https://github.com/tizianofiorenzani/ros_tutorials.git). For installing follow the tutorial here: https://www.leenabot.com/Servo-Motor-driver/ 4 | 5 | 6 | 7 | Servo number GPIO number Pin in P1 header 8 | 0 4 P1-7 9 | 1 17 P1-11 10 | 2 18 P1-12 11 | 3 21/27 P1-13 12 | 4 22 P1-15 13 | 5 23 P1-16 14 | 6 24 P1-18 15 | 7 25 P1-22 16 | 17 | P1-13 is connected to either GPIO-21 or GPIO-27, depending on board revision. 18 | 19 | after installation (just "make" and "make install") edit the config file 20 | /etc/init.d/servoblaster 21 | 22 | set in the OPTS="your_options". Mine are 23 | --idle-timeout=2000 --pcm --p1pins=0,0,0,0,15,16,0,0 24 | so you have 2 seconds timeout before setting to idle, pcm mode (rather than pwm) and only pins 15 and 16 set for the servo (gpio 22 and 23) 25 | 26 | tfmini.py 27 | Interface to tfmini laser 28 | 29 | 30 | tfmini_servo_scanner.py 31 | Class for a laser scanner 32 | -------------------------------------------------------------------------------- /laser_scanner_tfmini/src/ros_tfmini_laser_scanner.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from sensor_msgs.msg import LaserScan 5 | from tfmini_servo_scanner import * 6 | import math 7 | 8 | SERVO_GPIO = 23 9 | SRV_ANGLE_MIN = math.radians(-85) 10 | SRV_ANGLE_MAX = math.radians(85) 11 | SRV_DUTY_ANGLE_MIN = 2250 12 | SRV_DUTY_ANGLE_MAX = 750 13 | SRV_TIME_MIN_MAX = 0.4 14 | LASER_ANGLE_SAMPLES = 50 15 | 16 | 17 | def tfmini_laserscan_publisher(frame_id): 18 | 19 | scan_pup= rospy.Publisher('tfmini_laser', LaserScan, queue_size=0) 20 | 21 | scan = LaserScan() 22 | 23 | 24 | #-- Convention: counter clockwise is positive (left positive, right negative) 25 | tfminiscanner = TfminiServoScanner(SERVO_GPIO, SRV_ANGLE_MIN, SRV_ANGLE_MAX, 26 | SRV_DUTY_ANGLE_MIN, SRV_DUTY_ANGLE_MAX, LASER_ANGLE_SAMPLES, 27 | SRV_TIME_MIN_MAX) 28 | 29 | 30 | #-- Initialize the message 31 | scan.header.frame_id = frame_id; 32 | scan.range_min = tfminiscanner.laser.distance_min*0.01 33 | scan.range_max = tfminiscanner.laser.distance_max*0.01 34 | 35 | 36 | tfminiscanner.reset_servo() 37 | time.sleep(1) 38 | 39 | counter = 0 40 | 41 | while not rospy.is_shutdown(): 42 | ini_angle, end_angle, time_increment, angle_increment, ranges = tfminiscanner.scan(scale_factor=0.01, reset=True) 43 | scan.angle_min = ini_angle 44 | scan.angle_max = end_angle 45 | scan.angle_increment = angle_increment 46 | scan.time_increment = time_increment 47 | scan.ranges = ranges 48 | 49 | scan_pup.publish(scan) 50 | 51 | 52 | 53 | if __name__ == "__main__": 54 | rospy.init_node("tfmini_laserscan") 55 | tfmini_laserscan_publisher(frame_id="map") 56 | -------------------------------------------------------------------------------- /laser_scanner_tfmini/src/servoblaster.py: -------------------------------------------------------------------------------- 1 | <<<<<<< HEAD 2 | """ 3 | Servoblaster from https://github.com/richardghirst/PiBits/tree/master/ServoBlaster 4 | 5 | Servo number GPIO number Pin in P1 header 6 | 0 4 P1-7 7 | 1 17 P1-11 8 | 2 18 P1-12 9 | 3 21/27 P1-13 10 | 4 22 P1-15 11 | 5 23 P1-16 12 | 6 24 P1-18 13 | 7 25 P1-22 14 | 15 | Change the /etc/init.d/servoblaster file to OPTS="--idle-timeout=2000 --pcm --p1pins=" 16 | 17 | You can set the actual pin you want to dedicate to be servo with --p1pins option 18 | 19 | """ 20 | 21 | class ServoBlaster(): 22 | _servo_dict = { 4: 0, 23 | 17: 1, 24 | 18: 2, 25 | 21: 3, 26 | 22: 4, 27 | 23: 5, 28 | 24: 6, 29 | 25: 7} 30 | 31 | def __init__(self, gpio_port): 32 | 33 | self._gpio_port = gpio_port 34 | self._servo_number = self._servo_dict[gpio_port] 35 | 36 | def update(self, duty_us): 37 | servoStr ="%u=%u\n" % (self._servo_number, long(duty_us*0.1)) 38 | with open("/dev/servoblaster", "wb") as f: 39 | f.write(servoStr) 40 | 41 | class ServoAngle(): 42 | def __init__(self, gpio_port, angle_min, angle_max, min_duty, max_duty): 43 | self._gpio_port = gpio_port 44 | 45 | self._angle_to_duty = (max_duty - min_duty)/(angle_max - angle_min) 46 | self._duty_min = min_duty 47 | self._angle_min = angle_min 48 | self._angle_max = angle_max 49 | 50 | self._angle = 0 51 | 52 | self._servo = ServoBlaster(gpio_port) 53 | 54 | def angle_to_duty(self, angle): 55 | duty = (angle - self._angle_min)*self._angle_to_duty + self._duty_min 56 | return (duty) 57 | 58 | def update(self, angle): 59 | self._servo.update(self.angle_to_duty(angle)) 60 | self._angle = angle 61 | 62 | def set_to_min(self): 63 | self.update(self._angle_min) 64 | 65 | def set_to_max(self): 66 | self.update(self._angle_max) 67 | 68 | def set_to_middle(self): 69 | self.update((self._angle_min + self._angle_max)*0.5) 70 | 71 | @property 72 | def angle(self): 73 | return(self._angle) 74 | 75 | @property 76 | def angle_min(self): 77 | return(self._angle_min) 78 | 79 | @property 80 | def angle_max(self): 81 | return(self._angle_max) 82 | 83 | 84 | if __name__ == "__main__": 85 | import time 86 | #-- ROS Convention: counter clockwise is positive (left positive, right negative) 87 | servo_angle = ServoAngle(23, -85, 85, 2250, 750) 88 | 89 | angle = -85 90 | sleep = 0.01 91 | step = 2 92 | 93 | servo_angle.update(angle) 94 | time.sleep(1) 95 | 96 | while True: 97 | angle += step 98 | 99 | if angle > 85: angle = -85 100 | 101 | print ("%.0f"%angle) 102 | servo_angle.update(angle) 103 | 104 | if angle == -85: 105 | time.sleep(1) 106 | time.sleep(sleep) 107 | 108 | """ 109 | while True: 110 | print ("-45") 111 | servo_angle.update(-45) 112 | time.sleep(2) 113 | print ("0") 114 | servo_angle.update(0) 115 | time.sleep(2) 116 | print("+45") 117 | servo_angle.update(45) 118 | time.sleep(2) 119 | ======= 120 | """ 121 | Servoblaster from https://github.com/richardghirst/PiBits/tree/master/ServoBlaster 122 | 123 | Servo number GPIO number Pin in P1 header 124 | 0 4 P1-7 125 | 1 17 P1-11 126 | 2 18 P1-12 127 | 3 21/27 P1-13 128 | 4 22 P1-15 129 | 5 23 P1-16 130 | 6 24 P1-18 131 | 7 25 P1-22 132 | 133 | Change the /etc/init.d/servoblaster file to OPTS="--idle-timeout=2000 --pcm --p1pins=" 134 | 135 | You can set the actual pin you want to dedicate to be servo with --p1pins option 136 | 137 | """ 138 | 139 | class ServoBlaster(): 140 | _servo_dict = { 4: 0, 141 | 17: 1, 142 | 18: 2, 143 | 21: 3, 144 | 22: 4, 145 | 23: 5, 146 | 24: 6, 147 | 25: 7} 148 | 149 | def __init__(self, gpio_port): 150 | 151 | self._gpio_port = gpio_port 152 | self._servo_number = self._servo_dict[gpio_port] 153 | 154 | def update(self, duty_us): 155 | servoStr ="%u=%u\n" % (self._servo_number, long(duty_us*0.1)) 156 | with open("/dev/servoblaster", "wb") as f: 157 | f.write(servoStr) 158 | 159 | class ServoAngle(): 160 | def __init__(self, gpio_port, angle_min, angle_max, min_duty, max_duty): 161 | self._gpio_port = gpio_port 162 | 163 | self._angle_to_duty = (max_duty - min_duty)/(angle_max - angle_min) 164 | self._duty_min = min_duty 165 | self._angle_min = angle_min 166 | self._angle_max = angle_max 167 | 168 | self._angle = 0 169 | 170 | self._servo = ServoBlaster(gpio_port) 171 | 172 | def angle_to_duty(self, angle): 173 | duty = (angle - self._angle_min)*self._angle_to_duty + self._duty_min 174 | return (duty) 175 | 176 | def update(self, angle): 177 | self._servo.update(self.angle_to_duty(angle)) 178 | self._angle = angle 179 | 180 | def set_to_min(self): 181 | self.update(self._angle_min) 182 | 183 | def set_to_max(self): 184 | self.update(self._angle_max) 185 | 186 | def set_to_middle(self): 187 | self.update((self._angle_min + self._angle_max)*0.5) 188 | 189 | @property 190 | def angle(self): 191 | return(self._angle) 192 | 193 | @property 194 | def angle_min(self): 195 | return(self._angle_min) 196 | 197 | @property 198 | def angle_max(self): 199 | return(self._angle_max) 200 | 201 | 202 | if __name__ == "__main__": 203 | import time 204 | #-- ROS Convention: counter clockwise is positive (left positive, right negative) 205 | servo_angle = ServoAngle(23, -85, 85, 2250, 750) 206 | 207 | angle = -85 208 | sleep = 0.01 209 | step = 2 210 | 211 | servo_angle.update(angle) 212 | time.sleep(1) 213 | 214 | while True: 215 | angle += step 216 | 217 | if angle > 85: angle = -85 218 | 219 | print ("%.0f"%angle) 220 | servo_angle.update(angle) 221 | 222 | if angle == -85: 223 | time.sleep(1) 224 | time.sleep(sleep) 225 | 226 | """ 227 | while True: 228 | print ("-45") 229 | servo_angle.update(-45) 230 | time.sleep(2) 231 | print ("0") 232 | servo_angle.update(0) 233 | time.sleep(2) 234 | print("+45") 235 | servo_angle.update(45) 236 | time.sleep(2) 237 | >>>>>>> 339226fe7f5a20562103b4bc3e9d6b0802dcd869 238 | """ -------------------------------------------------------------------------------- /laser_scanner_tfmini/src/tfmini.py: -------------------------------------------------------------------------------- 1 | <<<<<<< HEAD 2 | # -*- coding: utf-8 -* 3 | import serial 4 | import time 5 | 6 | class TfMini(): 7 | 8 | def __init__(self, serial_port="/dev/ttyAMA0"): 9 | 10 | self._ser = serial.Serial(serial_port, 115200) 11 | if self._ser.is_open == False: 12 | self._ser.open() 13 | 14 | self._distance = 0 15 | self.distance_min = 10 16 | self.distance_max = 1200 17 | 18 | def get_data(self): 19 | 20 | time0 = time.time() 21 | while True: 22 | count = self._ser.in_waiting 23 | distance = -1 24 | if time.time() > time0 + 1: break 25 | if count > 8: 26 | recv = self._ser.read(9) 27 | self._ser.reset_input_buffer() 28 | if recv[0] == 'Y' and recv[1] == 'Y': # 0x59 is 'Y' 29 | low = int(recv[2].encode('hex'), 16) 30 | high = int(recv[3].encode('hex'), 16) 31 | distance = low + high * 256 32 | break 33 | 34 | self._distance = distance 35 | return(distance) 36 | 37 | @property 38 | def distance(self): 39 | return(self._distance) 40 | 41 | def print_data_thread(self): 42 | while True: 43 | print(self.get_data()) 44 | time.sleep(0.5) 45 | 46 | def close(self): 47 | if self._ != None: 48 | self._ser.close() 49 | 50 | 51 | 52 | if __name__ == '__main__': 53 | tfmini = TfMini() 54 | # print tfmini.get_data() 55 | tfmini.print_data_thread() 56 | ======= 57 | # -*- coding: utf-8 -* 58 | #-- Derived from https://github.com/TFmini/TFmini-RaspberryPi 59 | 60 | import serial 61 | import time 62 | 63 | class TfMini(): 64 | 65 | def __init__(self, serial_port="/dev/ttyAMA0"): 66 | 67 | self._ser = serial.Serial(serial_port, 115200) 68 | if self._ser.is_open == False: 69 | self._ser.open() 70 | 71 | self._distance = 0 72 | self.distance_min = 10 73 | self.distance_max = 1200 74 | 75 | def get_data(self): 76 | 77 | time0 = time.time() 78 | while True: 79 | count = self._ser.in_waiting 80 | distance = -1 81 | if time.time() > time0 + 1: break 82 | if count > 8: 83 | recv = self._ser.read(9) 84 | self._ser.reset_input_buffer() 85 | if recv[0] == 'Y' and recv[1] == 'Y': # 0x59 is 'Y' 86 | low = int(recv[2].encode('hex'), 16) 87 | high = int(recv[3].encode('hex'), 16) 88 | distance = low + high * 256 89 | break 90 | 91 | self._distance = distance 92 | return(distance) 93 | 94 | @property 95 | def distance(self): 96 | return(self._distance) 97 | 98 | def print_data_thread(self): 99 | while True: 100 | print(self.get_data()) 101 | time.sleep(0.5) 102 | 103 | def close(self): 104 | if self._ != None: 105 | self._ser.close() 106 | 107 | 108 | 109 | if __name__ == '__main__': 110 | tfmini = TfMini() 111 | # print tfmini.get_data() 112 | tfmini.print_data_thread() 113 | >>>>>>> 339226fe7f5a20562103b4bc3e9d6b0802dcd869 114 | -------------------------------------------------------------------------------- /laser_scanner_tfmini/src/tfmini_servo_scanner.py: -------------------------------------------------------------------------------- 1 | <<<<<<< HEAD 2 | 3 | import servoblaster as sb 4 | import tfmini 5 | import time 6 | import math 7 | 8 | class TfminiServoScanner(): 9 | def __init__(self, 10 | servo_gpio, #- [ ] Gpio port assigned to the servo 11 | angle_min=-90, #- [deg] Minimum angle of the servo 12 | angle_max=90, #- [deg] Maximum angle of the servo 13 | duty_min=1000, #- [us] duty cycle corresponding to minimum angle 14 | duty_max=2000, #- [us] duty cycle corresponding to minimum angle 15 | n_steps=10, #- [ ] number of measurements in the min-max range 16 | time_min_max=0.5, #- [s] time for the servo to move from min to max 17 | serial_port="/dev/ttyAMA0" # serial port for tfmini 18 | ): 19 | 20 | #-- Create an object servo 21 | self.servo = sb.ServoAngle(servo_gpio, angle_min, angle_max, duty_min, duty_max) 22 | 23 | #-- Create an object laser 24 | self.laser = tfmini.TfMini(serial_port) 25 | 26 | #-- Calculate the angle step 27 | self._delta_angle = (angle_max - angle_min)/(n_steps - 1) 28 | 29 | #-- Calculate the servo speed 30 | self._time_min_max = time_min_max 31 | self._servo_speed = (angle_max - angle_min)/time_min_max 32 | 33 | #-- Calculate the minimum pause after each step command 34 | self._min_time_pause = self._delta_angle/self._servo_speed 35 | 36 | #-- initialize the rotational direction to 1 37 | self._move_dir = 1; 38 | 39 | def read_laser(self): #- Read the laser and return the value 40 | return(self.laser.get_data()) 41 | 42 | def reset_servo(self): #- Set the servo to min angle position 43 | self.servo.set_to_min() 44 | self._move_dir = 1; 45 | time.sleep(self._time_min_max) 46 | 47 | def move_servo(self): #- move the servo of one step 48 | angle = self.angle 49 | 50 | #-- When reached the end, change direction 51 | if angle >= self.servo.angle_max - self._delta_angle: 52 | self._move_dir = -1 53 | 54 | if angle <= self.servo.angle_min + self._delta_angle: 55 | self._move_dir = 1 56 | 57 | #- Get the commanded angle 58 | angle += self._delta_angle*self._move_dir 59 | 60 | #- Command the servo 61 | self.servo.update(angle) 62 | 63 | #- Sleep 64 | time.sleep(self._min_time_pause) 65 | 66 | def scan(self, scale_factor=1.0, reset=False): 67 | if reset: self.reset_servo() 68 | 69 | ini_angle = self.angle 70 | 71 | self.servo.update(ini_angle) 72 | 73 | ranges = [] 74 | angle = ini_angle 75 | move_dir = self._move_dir 76 | time_init = time.time() 77 | 78 | while True: 79 | dist = self.read_laser()*scale_factor 80 | angle = self.angle 81 | print("d = %4.2f a = %4.2f"%(dist, self.angle)) 82 | ranges.append(dist) 83 | 84 | self.move_servo() 85 | 86 | #-- If changed sign: break 87 | if move_dir*self._move_dir < 0: 88 | break 89 | 90 | time_increment = (time.time() - time_init)/(len(ranges) - 1) 91 | angle_increment = (angle - ini_angle)/(len(ranges) - 1 ) 92 | 93 | return(ini_angle, angle, time_increment, angle_increment, ranges) 94 | 95 | @property 96 | def angle(self): 97 | return(self.servo.angle) 98 | 99 | @property 100 | def time_between_measurements(self): 101 | return(self._min_time_pause) 102 | 103 | @property 104 | def step(self): 105 | return(self._delta_angle) 106 | 107 | 108 | if __name__ == "__main__": 109 | #-- Convention: counter clockwise is positive (left positive, right negative) 110 | tfminiscanner = TfminiServoScanner(23, -85, 85, 2250, 750, 20, 0.5) 111 | 112 | tfminiscanner.reset_servo() 113 | time.sleep(1) 114 | 115 | while True: 116 | print tfminiscanner.scan(reset=True) 117 | 118 | """ 119 | while True: 120 | tfminiscanner.move_servo() 121 | dist = tfminiscanner.read_laser() 122 | print("d = %4d a = %4d"%(dist, tfminiscanner.angle)) 123 | 124 | """ 125 | ======= 126 | 127 | import servoblaster as sb 128 | import tfmini 129 | import time 130 | import math 131 | 132 | class TfminiServoScanner(): 133 | def __init__(self, 134 | servo_gpio, #- [ ] Gpio port assigned to the servo 135 | angle_min=-90, #- [deg] Minimum angle of the servo 136 | angle_max=90, #- [deg] Maximum angle of the servo 137 | duty_min=1000, #- [us] duty cycle corresponding to minimum angle 138 | duty_max=2000, #- [us] duty cycle corresponding to minimum angle 139 | n_steps=10, #- [ ] number of measurements in the min-max range 140 | time_min_max=0.5, #- [s] time for the servo to move from min to max 141 | serial_port="/dev/ttyAMA0" # serial port for tfmini 142 | ): 143 | 144 | #-- Create an object servo 145 | self.servo = sb.ServoAngle(servo_gpio, angle_min, angle_max, duty_min, duty_max) 146 | 147 | #-- Create an object laser 148 | self.laser = tfmini.TfMini(serial_port) 149 | 150 | #-- Calculate the angle step 151 | self._delta_angle = (angle_max - angle_min)/(n_steps - 1) 152 | 153 | #-- Calculate the servo speed 154 | self._time_min_max = time_min_max 155 | self._servo_speed = (angle_max - angle_min)/time_min_max 156 | 157 | #-- Calculate the minimum pause after each step command 158 | self._min_time_pause = self._delta_angle/self._servo_speed 159 | 160 | #-- initialize the rotational direction to 1 161 | self._move_dir = 1; 162 | 163 | def read_laser(self): #- Read the laser and return the value 164 | return(self.laser.get_data()) 165 | 166 | def reset_servo(self): #- Set the servo to min angle position 167 | self.servo.set_to_min() 168 | self._move_dir = 1; 169 | time.sleep(self._time_min_max) 170 | 171 | def move_servo(self): #- move the servo of one step 172 | angle = self.angle 173 | 174 | #-- When reached the end, change direction 175 | if angle >= self.servo.angle_max - self._delta_angle: 176 | self._move_dir = -1 177 | 178 | if angle <= self.servo.angle_min + self._delta_angle: 179 | self._move_dir = 1 180 | 181 | #- Get the commanded angle 182 | angle += self._delta_angle*self._move_dir 183 | 184 | #- Command the servo 185 | self.servo.update(angle) 186 | 187 | #- Sleep 188 | time.sleep(self._min_time_pause) 189 | 190 | def scan(self, scale_factor=1.0, reset=False): 191 | if reset: self.reset_servo() 192 | 193 | ini_angle = self.angle 194 | 195 | self.servo.update(ini_angle) 196 | 197 | ranges = [] 198 | angle = ini_angle 199 | move_dir = self._move_dir 200 | time_init = time.time() 201 | 202 | while True: 203 | dist = self.read_laser()*scale_factor 204 | angle = self.angle 205 | print("d = %4.2f a = %4.2f"%(dist, self.angle)) 206 | ranges.append(dist) 207 | 208 | self.move_servo() 209 | 210 | #-- If changed sign: break 211 | if move_dir*self._move_dir < 0: 212 | break 213 | 214 | time_increment = (time.time() - time_init)/(len(ranges) - 1) 215 | angle_increment = (angle - ini_angle)/(len(ranges) - 1 ) 216 | 217 | return(ini_angle, angle, time_increment, angle_increment, ranges) 218 | 219 | @property 220 | def angle(self): 221 | return(self.servo.angle) 222 | 223 | @property 224 | def time_between_measurements(self): 225 | return(self._min_time_pause) 226 | 227 | @property 228 | def step(self): 229 | return(self._delta_angle) 230 | 231 | 232 | if __name__ == "__main__": 233 | #-- Convention: counter clockwise is positive (left positive, right negative) 234 | tfminiscanner = TfminiServoScanner(23, -85, 85, 2250, 750, 20, 0.5) 235 | 236 | tfminiscanner.reset_servo() 237 | time.sleep(1) 238 | 239 | while True: 240 | print tfminiscanner.scan(reset=True) 241 | 242 | """ 243 | while True: 244 | tfminiscanner.move_servo() 245 | dist = tfminiscanner.read_laser() 246 | print("d = %4d a = %4d"%(dist, tfminiscanner.angle)) 247 | 248 | """ 249 | >>>>>>> 339226fe7f5a20562103b4bc3e9d6b0802dcd869 250 | -------------------------------------------------------------------------------- /opencv/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(opencv) 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 | cv_bridge 12 | image_transport 13 | rospy 14 | sensor_msgs 15 | std_msgs 16 | ) 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 exec_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 exec_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 | # sensor_msgs# std_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 exec_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 opencv_helloworld 110 | # CATKIN_DEPENDS cv_bridge image_transport rospy sensor_msgs std_msgs 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}/opencv_helloworld.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/opencv_helloworld_node.cpp) 139 | 140 | ## Rename C++ executable without prefix 141 | ## The above recommended prefix causes long target names, the following renames the 142 | ## target back to the shorter version for ease of user use 143 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 144 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 145 | 146 | ## Add cmake target dependencies of the executable 147 | ## same as for the library above 148 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 149 | 150 | ## Specify libraries to link a library or executable target against 151 | # target_link_libraries(${PROJECT_NAME}_node 152 | # ${catkin_LIBRARIES} 153 | # ) 154 | 155 | ############# 156 | ## Install ## 157 | ############# 158 | 159 | # all install targets should use catkin DESTINATION variables 160 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 161 | 162 | ## Mark executable scripts (Python etc.) for installation 163 | ## in contrast to setup.py, you can choose the destination 164 | # install(PROGRAMS 165 | # scripts/my_python_script 166 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark executables and/or libraries for installation 170 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 171 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 172 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 174 | # ) 175 | 176 | ## Mark cpp header files for installation 177 | # install(DIRECTORY include/${PROJECT_NAME}/ 178 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 179 | # FILES_MATCHING PATTERN "*.h" 180 | # PATTERN ".svn" EXCLUDE 181 | # ) 182 | 183 | ## Mark other files for installation (e.g. launch and bag files, etc.) 184 | # install(FILES 185 | # # myfile1 186 | # # myfile2 187 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 188 | # ) 189 | 190 | ############# 191 | ## Testing ## 192 | ############# 193 | 194 | ## Add gtest based cpp test target and link libraries 195 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_opencv_helloworld.cpp) 196 | # if(TARGET ${PROJECT_NAME}-test) 197 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 198 | # endif() 199 | 200 | ## Add folders to be run by python nosetests 201 | # catkin_add_nosetests(test) 202 | -------------------------------------------------------------------------------- /opencv/README.txt: -------------------------------------------------------------------------------- 1 | Folder "include": contains libraries and blob detection tests 2 | Folder "src": contains ROS nodes 3 | -------------------------------------------------------------------------------- /opencv/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/opencv/__init__.py -------------------------------------------------------------------------------- /opencv/include/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/opencv/include/__init__.py -------------------------------------------------------------------------------- /opencv/include/blob.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/opencv/include/blob.jpg -------------------------------------------------------------------------------- /opencv/include/blob2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/opencv/include/blob2.jpg -------------------------------------------------------------------------------- /opencv/include/blob3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/opencv/include/blob3.jpg -------------------------------------------------------------------------------- /opencv/include/blob_detector.py: -------------------------------------------------------------------------------- 1 | """ 2 | Library for detecting a blob based on a color range filter in HSV space 3 | 4 | 0------------------> x (cols) 5 | | 6 | | 7 | | o center 8 | | 9 | | 10 | V y (rows) 11 | 12 | Author: Tiziano Fiorenzani 13 | 14 | """ 15 | 16 | 17 | # Standard imports 18 | import cv2 19 | import numpy as np; 20 | 21 | #---------- Blob detecting function: returns keypoints and mask 22 | #-- return keypoints, reversemask 23 | def blob_detect(image, #-- The frame (cv standard) 24 | hsv_min, #-- minimum threshold of the hsv filter [h_min, s_min, v_min] 25 | hsv_max, #-- maximum threshold of the hsv filter [h_max, s_max, v_max] 26 | blur=0, #-- blur value (default 0) 27 | blob_params=None, #-- blob parameters (default None) 28 | search_window=None, #-- window where to search as [x_min, y_min, x_max, y_max] adimensional (0.0 to 1.0) starting from top left corner 29 | imshow=False 30 | ): 31 | 32 | #- Blur image to remove noise 33 | if blur > 0: 34 | image = cv2.blur(image, (blur, blur)) 35 | #- Show result 36 | if imshow: 37 | cv2.imshow("Blur", image) 38 | cv2.waitKey(0) 39 | 40 | #- Search window 41 | if search_window is None: search_window = [0.0, 0.0, 1.0, 1.0] 42 | 43 | #- Convert image from BGR to HSV 44 | hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) 45 | 46 | #- Apply HSV threshold 47 | mask = cv2.inRange(hsv,hsv_min, hsv_max) 48 | 49 | #- Show HSV Mask 50 | if imshow: 51 | cv2.imshow("HSV Mask", mask) 52 | 53 | #- dilate makes the in range areas larger 54 | mask = cv2.dilate(mask, None, iterations=2) 55 | #- Show HSV Mask 56 | if imshow: 57 | cv2.imshow("Dilate Mask", mask) 58 | cv2.waitKey(0) 59 | 60 | mask = cv2.erode(mask, None, iterations=2) 61 | 62 | #- Show dilate/erode mask 63 | if imshow: 64 | cv2.imshow("Erode Mask", mask) 65 | cv2.waitKey(0) 66 | 67 | #- Cut the image using the search mask 68 | mask = apply_search_window(mask, search_window) 69 | 70 | if imshow: 71 | cv2.imshow("Searching Mask", mask) 72 | cv2.waitKey(0) 73 | 74 | #- build default blob detection parameters, if none have been provided 75 | if blob_params is None: 76 | # Set up the SimpleBlobdetector with default parameters. 77 | params = cv2.SimpleBlobDetector_Params() 78 | 79 | # Change thresholds 80 | params.minThreshold = 0; 81 | params.maxThreshold = 100; 82 | 83 | # Filter by Area. 84 | params.filterByArea = True 85 | params.minArea = 30 86 | params.maxArea = 20000 87 | 88 | # Filter by Circularity 89 | params.filterByCircularity = True 90 | params.minCircularity = 0.1 91 | 92 | # Filter by Convexity 93 | params.filterByConvexity = True 94 | params.minConvexity = 0.5 95 | 96 | # Filter by Inertia 97 | params.filterByInertia =True 98 | params.minInertiaRatio = 0.5 99 | 100 | else: 101 | params = blob_params 102 | 103 | #- Apply blob detection 104 | detector = cv2.SimpleBlobDetector_create(params) 105 | 106 | # Reverse the mask: blobs are black on white 107 | reversemask = 255-mask 108 | 109 | if imshow: 110 | cv2.imshow("Reverse Mask", reversemask) 111 | cv2.waitKey(0) 112 | 113 | keypoints = detector.detect(reversemask) 114 | 115 | return keypoints, reversemask 116 | 117 | #---------- Draw detected blobs: returns the image 118 | #-- return(im_with_keypoints) 119 | def draw_keypoints(image, #-- Input image 120 | keypoints, #-- CV keypoints 121 | line_color=(0,0,255), #-- line's color (b,g,r) 122 | imshow=False #-- show the result 123 | ): 124 | 125 | #-- Draw detected blobs as red circles. 126 | #-- cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS ensures the size of the circle corresponds to the size of blob 127 | im_with_keypoints = cv2.drawKeypoints(image, keypoints, np.array([]), line_color, cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS) 128 | 129 | if imshow: 130 | # Show keypoints 131 | cv2.imshow("Keypoints", im_with_keypoints) 132 | 133 | return(im_with_keypoints) 134 | 135 | #---------- Draw search window: returns the image 136 | #-- return(image) 137 | def draw_window(image, #- Input image 138 | window_adim, #- window in adimensional units 139 | color=(255,0,0), #- line's color 140 | line=5, #- line's thickness 141 | imshow=False #- show the image 142 | ): 143 | 144 | rows = image.shape[0] 145 | cols = image.shape[1] 146 | 147 | x_min_px = int(cols*window_adim[0]) 148 | y_min_px = int(rows*window_adim[1]) 149 | x_max_px = int(cols*window_adim[2]) 150 | y_max_px = int(rows*window_adim[3]) 151 | 152 | #-- Draw a rectangle from top left to bottom right corner 153 | image = cv2.rectangle(image,(x_min_px,y_min_px),(x_max_px,y_max_px),color,line) 154 | 155 | if imshow: 156 | # Show keypoints 157 | cv2.imshow("Keypoints", image) 158 | 159 | return(image) 160 | 161 | #---------- Draw X Y frame 162 | #-- return(image) 163 | def draw_frame(image, 164 | dimension=0.3, #- dimension relative to frame size 165 | line=2 #- line's thickness 166 | ): 167 | 168 | rows = image.shape[0] 169 | cols = image.shape[1] 170 | size = min([rows, cols]) 171 | center_x = int(cols/2.0) 172 | center_y = int(rows/2.0) 173 | 174 | line_length = int(size*dimension) 175 | 176 | #-- X 177 | image = cv2.line(image, (center_x, center_y), (center_x+line_length, center_y), (0,0,255), line) 178 | #-- Y 179 | image = cv2.line(image, (center_x, center_y), (center_x, center_y+line_length), (0,255,0), line) 180 | 181 | return (image) 182 | 183 | #---------- Apply search window: returns the image 184 | #-- return(image) 185 | def apply_search_window(image, window_adim=[0.0, 0.0, 1.0, 1.0]): 186 | rows = image.shape[0] 187 | cols = image.shape[1] 188 | x_min_px = int(cols*window_adim[0]) 189 | y_min_px = int(rows*window_adim[1]) 190 | x_max_px = int(cols*window_adim[2]) 191 | y_max_px = int(rows*window_adim[3]) 192 | 193 | #--- Initialize the mask as a black image 194 | mask = np.zeros(image.shape,np.uint8) 195 | 196 | #--- Copy the pixels from the original image corresponding to the window 197 | mask[y_min_px:y_max_px,x_min_px:x_max_px] = image[y_min_px:y_max_px,x_min_px:x_max_px] 198 | 199 | #--- return the mask 200 | return(mask) 201 | 202 | #---------- Apply a blur to the outside search region 203 | #-- return(image) 204 | def blur_outside(image, blur=5, window_adim=[0.0, 0.0, 1.0, 1.0]): 205 | rows = image.shape[0] 206 | cols = image.shape[1] 207 | x_min_px = int(cols*window_adim[0]) 208 | y_min_px = int(rows*window_adim[1]) 209 | x_max_px = int(cols*window_adim[2]) 210 | y_max_px = int(rows*window_adim[3]) 211 | 212 | #--- Initialize the mask as a black image 213 | mask = cv2.blur(image, (blur, blur)) 214 | 215 | #--- Copy the pixels from the original image corresponding to the window 216 | mask[y_min_px:y_max_px,x_min_px:x_max_px] = image[y_min_px:y_max_px,x_min_px:x_max_px] 217 | 218 | 219 | 220 | #--- return the mask 221 | return(mask) 222 | 223 | #---------- Obtain the camera relative frame coordinate of one single keypoint 224 | #-- return(x,y) 225 | def get_blob_relative_position(image, keyPoint): 226 | rows = float(image.shape[0]) 227 | cols = float(image.shape[1]) 228 | # print(rows, cols) 229 | center_x = 0.5*cols 230 | center_y = 0.5*rows 231 | # print(center_x) 232 | x = (keyPoint.pt[0] - center_x)/(center_x) 233 | y = (keyPoint.pt[1] - center_y)/(center_y) 234 | return(x,y) 235 | 236 | 237 | 238 | #----------- TEST 239 | if __name__=="__main__": 240 | 241 | #--- Define HSV limits 242 | blue_min = (77,40,0) 243 | blue_max = (101, 255, 255) 244 | 245 | #--- Define area limit [x_min, y_min, x_max, y_max] adimensional (0.0 to 1.0) starting from top left corner 246 | window = [0.25, 0.25, 0.65, 0.75] 247 | 248 | #-- IMAGE_SOURCE: either 'camera' or 'imagelist' 249 | #SOURCE = 'video' 250 | SOURCE = 'camera' 251 | 252 | if SOURCE == 'video': 253 | cap = cv2.VideoCapture(0) 254 | while(True): 255 | # Capture frame-by-frame 256 | ret, frame = cap.read() 257 | 258 | #-- Detect keypoints 259 | keypoints, _ = blob_detect(frame, blue_min, blue_max, blur=3, 260 | blob_params=None, search_window=window, imshow=False) 261 | #-- Draw search window 262 | frame = draw_window(frame, window) 263 | 264 | #-- click ENTER on the image window to proceed 265 | draw_keypoints(frame, keypoints, imshow=True) 266 | 267 | #-- press q to quit 268 | if cv2.waitKey(1) & 0xFF == ord('q'): 269 | break 270 | 271 | else: 272 | #-- Read image list from file: 273 | image_list = [] 274 | image_list.append(cv2.imread("blob.jpg")) 275 | #image_list.append(cv2.imread("blob2.jpg")) 276 | #image_list.append(cv2.imread("blob3.jpg")) 277 | 278 | for image in image_list: 279 | #-- Detect keypoints 280 | keypoints, _ = blob_detect(image, blue_min, blue_max, blur=5, 281 | blob_params=None, search_window=window, imshow=True) 282 | 283 | image = blur_outside(image, blur=15, window_adim=window) 284 | cv2.imshow("Outside Blur", image) 285 | cv2.waitKey(0) 286 | 287 | image = draw_window(image, window, imshow=True) 288 | #-- enter to proceed 289 | cv2.waitKey(0) 290 | 291 | #-- click ENTER on the image window to proceed 292 | image = draw_keypoints(image, keypoints, imshow=True) 293 | cv2.waitKey(0) 294 | #-- Draw search window 295 | 296 | image = draw_frame(image) 297 | cv2.imshow("Frame", image) 298 | cv2.waitKey(0) 299 | 300 | 301 | -------------------------------------------------------------------------------- /opencv/include/range_detector.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | #-- COPIED FROM IMUTILS LIBRARY: https://github.com/jrosebr1/imutils/blob/master/bin/range-detector 5 | 6 | 7 | # USAGE: You need to specify a filter and "only one" image source 8 | # 9 | # (python) range-detector --filter RGB --image /path/to/image.png 10 | # or 11 | # (python) range-detector --filter HSV --webcam 12 | 13 | import cv2 14 | import argparse 15 | from operator import xor 16 | 17 | 18 | def callback(value): 19 | pass 20 | 21 | 22 | def setup_trackbars(range_filter): 23 | cv2.namedWindow("Trackbars", 0) 24 | 25 | for i in ["MIN", "MAX"]: 26 | v = 0 if i == "MIN" else 255 27 | 28 | for j in range_filter: 29 | cv2.createTrackbar("%s_%s" % (j, i), "Trackbars", v, 255, callback) 30 | 31 | 32 | def get_arguments(): 33 | ap = argparse.ArgumentParser() 34 | ap.add_argument('-f', '--filter', required=True, 35 | help='Range filter. RGB or HSV') 36 | ap.add_argument('-i', '--image', required=False, 37 | help='Path to the image') 38 | ap.add_argument('-w', '--webcam', required=False, 39 | help='Use webcam', action='store_true') 40 | ap.add_argument('-p', '--preview', required=False, 41 | help='Show a preview of the image after applying the mask', 42 | action='store_true') 43 | args = vars(ap.parse_args()) 44 | 45 | if not xor(bool(args['image']), bool(args['webcam'])): 46 | ap.error("Please specify only one image source") 47 | 48 | if not args['filter'].upper() in ['RGB', 'HSV']: 49 | ap.error("Please speciy a correct filter.") 50 | 51 | return args 52 | 53 | 54 | def get_trackbar_values(range_filter): 55 | values = [] 56 | 57 | for i in ["MIN", "MAX"]: 58 | for j in range_filter: 59 | v = cv2.getTrackbarPos("%s_%s" % (j, i), "Trackbars") 60 | values.append(v) 61 | 62 | return values 63 | 64 | 65 | def main(): 66 | args = get_arguments() 67 | 68 | range_filter = args['filter'].upper() 69 | 70 | if args['image']: 71 | image = cv2.imread(args['image']) 72 | 73 | if range_filter == 'RGB': 74 | frame_to_thresh = image.copy() 75 | else: 76 | frame_to_thresh = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) 77 | else: 78 | camera = cv2.VideoCapture(0) 79 | 80 | setup_trackbars(range_filter) 81 | 82 | while True: 83 | if args['webcam']: 84 | ret, image = camera.read() 85 | 86 | if not ret: 87 | break 88 | 89 | if range_filter == 'RGB': 90 | frame_to_thresh = image.copy() 91 | else: 92 | frame_to_thresh = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) 93 | 94 | v1_min, v2_min, v3_min, v1_max, v2_max, v3_max = get_trackbar_values(range_filter) 95 | 96 | thresh = cv2.inRange(frame_to_thresh, (v1_min, v2_min, v3_min), (v1_max, v2_max, v3_max)) 97 | 98 | if args['preview']: 99 | preview = cv2.bitwise_and(image, image, mask=thresh) 100 | cv2.imshow("Preview", preview) 101 | else: 102 | cv2.imshow("Original", image) 103 | cv2.imshow("Thresh", thresh) 104 | 105 | if cv2.waitKey(1) & 0xFF is ord('q'): 106 | break 107 | 108 | 109 | if __name__ == '__main__': 110 | main() 111 | 112 | -------------------------------------------------------------------------------- /opencv/launch/camerav2_320x240.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /opencv/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | opencv 4 | 0.0.0 5 | The opencv_helloworld package 6 | 7 | 8 | 9 | 10 | tiffo 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 | cv_bridge 53 | image_transport 54 | rospy 55 | sensor_msgs 56 | std_msgs 57 | cv_bridge 58 | image_transport 59 | rospy 60 | sensor_msgs 61 | std_msgs 62 | cv_bridge 63 | image_transport 64 | rospy 65 | sensor_msgs 66 | std_msgs 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /opencv/src/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/opencv/src/__init__.py -------------------------------------------------------------------------------- /opencv/src/find_ball.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | ON THE RASPI: roslaunch raspicam_node camerav2_320x240.launch enable_raw:=true 5 | 6 | 0------------------> x (cols) Image Frame 7 | | 8 | | c Camera frame 9 | | o---> x 10 | | | 11 | | V y 12 | | 13 | V y (rows) 14 | 15 | 16 | SUBSCRIBES TO: 17 | /raspicam_node/image: Source image topic 18 | 19 | PUBLISHES TO: 20 | /blob/image_blob : image with detected blob and search window 21 | /blob/image_mask : masking 22 | /blob/point_blob : blob position in adimensional values wrt. camera frame 23 | 24 | """ 25 | 26 | 27 | #--- Allow relative importing 28 | if __name__ == '__main__' and __package__ is None: 29 | from os import sys, path 30 | sys.path.append(path.dirname(path.dirname(path.abspath(__file__)))) 31 | 32 | import sys 33 | import rospy 34 | import cv2 35 | import time 36 | 37 | from std_msgs.msg import String 38 | from sensor_msgs.msg import Image 39 | from geometry_msgs.msg import Point 40 | from cv_bridge import CvBridge, CvBridgeError 41 | from include.blob_detector import * 42 | 43 | 44 | class BlobDetector: 45 | 46 | def __init__(self, thr_min, thr_max, blur=15, blob_params=None, detection_window=None): 47 | 48 | self.set_threshold(thr_min, thr_max) 49 | self.set_blur(blur) 50 | self.set_blob_params(blob_params) 51 | self.detection_window = detection_window 52 | 53 | self._t0 = time.time() 54 | 55 | self.blob_point = Point() 56 | 57 | print (">> Publishing image to topic image_blob") 58 | self.image_pub = rospy.Publisher("/blob/image_blob",Image,queue_size=1) 59 | self.mask_pub = rospy.Publisher("/blob/image_mask",Image,queue_size=1) 60 | print (">> Publishing position to topic point_blob") 61 | self.blob_pub = rospy.Publisher("/blob/point_blob",Point,queue_size=1) 62 | 63 | self.bridge = CvBridge() 64 | self.image_sub = rospy.Subscriber("/raspicam_node/image",Image,self.callback) 65 | print ("<< Subscribed to topic /raspicam_node/image") 66 | 67 | def set_threshold(self, thr_min, thr_max): 68 | self._threshold = [thr_min, thr_max] 69 | 70 | def set_blur(self, blur): 71 | self._blur = blur 72 | 73 | def set_blob_params(self, blob_params): 74 | self._blob_params = blob_params 75 | 76 | # def get_blob_relative_position(self, cv_image, keyPoint): 77 | # rows = float(cv_image.shape[0]) 78 | # cols = float(cv_image.shape[1]) 79 | # # print(rows, cols) 80 | # center_x = 0.5*cols 81 | # center_y = 0.5*rows 82 | # # print(center_x) 83 | # x = (keyPoint.pt[0] - center_x)/(center_x) 84 | # y = (keyPoint.pt[1] - center_y)/(center_y) 85 | # return(x,y) 86 | 87 | 88 | def callback(self,data): 89 | #--- Assuming image is 320x240 90 | try: 91 | cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") 92 | except CvBridgeError as e: 93 | print(e) 94 | 95 | (rows,cols,channels) = cv_image.shape 96 | if cols > 60 and rows > 60 : 97 | #--- Detect blobs 98 | keypoints, mask = blob_detect(cv_image, self._threshold[0], self._threshold[1], self._blur, 99 | blob_params=self._blob_params, search_window=self.detection_window ) 100 | #--- Draw search window and blobs 101 | cv_image = blur_outside(cv_image, 10, self.detection_window) 102 | 103 | cv_image = draw_window(cv_image, self.detection_window, line=1) 104 | cv_image = draw_frame(cv_image) 105 | 106 | cv_image = draw_keypoints(cv_image, keypoints) 107 | 108 | try: 109 | self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8")) 110 | self.mask_pub.publish(self.bridge.cv2_to_imgmsg(mask, "8UC1")) 111 | except CvBridgeError as e: 112 | print(e) 113 | 114 | for i, keyPoint in enumerate(keypoints): 115 | #--- Here you can implement some tracking algorithm to filter multiple detections 116 | #--- We are simply getting the first result 117 | x = keyPoint.pt[0] 118 | y = keyPoint.pt[1] 119 | s = keyPoint.size 120 | print ("kp %d: s = %3d x = %3d y= %3d"%(i, s, x, y)) 121 | 122 | #--- Find x and y position in camera adimensional frame 123 | x, y = get_blob_relative_position(cv_image, keyPoint) 124 | 125 | self.blob_point.x = x 126 | self.blob_point.y = y 127 | self.blob_pub.publish(self.blob_point) 128 | break 129 | 130 | fps = 1.0/(time.time()-self._t0) 131 | self._t0 = time.time() 132 | 133 | def main(args): 134 | blue_min = (77,40,0) 135 | blue_max = (101, 255, 255) 136 | # blue_min = (82,31,62) 137 | # blue_max = (106, 116, 193) 138 | blue_min = (55,40,0) 139 | blue_max = (150, 255, 255) 140 | 141 | blur = 5 142 | min_size = 10 143 | max_size = 40 144 | 145 | #--- detection window respect to camera frame in [x_min, y_min, x_max, y_max] adimensional (0 to 1) 146 | x_min = 0.1 147 | x_max = 0.9 148 | y_min = 0.4 149 | y_max = 0.9 150 | 151 | detection_window = [x_min, y_min, x_max, y_max] 152 | 153 | params = cv2.SimpleBlobDetector_Params() 154 | 155 | # Change thresholds 156 | params.minThreshold = 0; 157 | params.maxThreshold = 100; 158 | 159 | # Filter by Area. 160 | params.filterByArea = True 161 | params.minArea = 20 162 | params.maxArea = 20000 163 | 164 | # Filter by Circularity 165 | params.filterByCircularity = True 166 | params.minCircularity = 0.1 167 | 168 | # Filter by Convexity 169 | params.filterByConvexity = True 170 | params.minConvexity = 0.2 171 | 172 | # Filter by Inertia 173 | params.filterByInertia = True 174 | params.minInertiaRatio = 0.7 175 | 176 | ic = BlobDetector(blue_min, blue_max, blur, params, detection_window) 177 | rospy.init_node('blob_detector', anonymous=True) 178 | try: 179 | rospy.spin() 180 | except KeyboardInterrupt: 181 | print("Shutting down") 182 | 183 | cv2.destroyAllWindows() 184 | 185 | if __name__ == '__main__': 186 | main(sys.argv) 187 | -------------------------------------------------------------------------------- /opencv/src/test_cv_bridge.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # SIMPLE SCRIPT TO TEST CV_BRIDGE PACKAGE 4 | # 5 | #- ON THE RASPI: roslaunch raspicam_node camerav2_1280x960.launch enable_raw:=true 6 | # 7 | 8 | import sys 9 | 10 | import rospy 11 | import cv2 12 | 13 | from sensor_msgs.msg import Image 14 | from cv_bridge import CvBridge, CvBridgeError 15 | 16 | #--- Define our Class 17 | class image_converter: 18 | 19 | def __init__(self): 20 | #--- Publisher of the edited frame 21 | self.image_pub = rospy.Publisher("image_topic",Image,queue_size=1) 22 | 23 | #--- Subscriber to the camera flow 24 | self.bridge = CvBridge() 25 | self.image_sub = rospy.Subscriber("/raspicam_node/image",Image,self.callback) 26 | 27 | def callback(self,data): #--- Callback function 28 | 29 | #--- Read the frame and convert it using bridge 30 | try: 31 | cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") 32 | except CvBridgeError as e: 33 | print(e) 34 | 35 | #--- If a valid frame is received, draw a circle and write HELLO WORLD 36 | (rows,cols,channels) = cv_image.shape 37 | if cols > 20 and rows > 20: 38 | #--- Circle 39 | cv2.circle(cv_image, (500,500), 200, 255) 40 | 41 | #--- Text 42 | text = "HELLO WORLD" 43 | cv2.putText(cv_image, text, (100, 150), cv2.FONT_HERSHEY_SIMPLEX, 5, [0,0,200], 5) 44 | 45 | #--- Optional: show the image on a window (comment this for the Raspberry Pi) 46 | cv2.imshow("Image window", cv_image) 47 | cv2.waitKey(3) 48 | 49 | #--- Publish the modified frame to a new topic 50 | try: 51 | self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8")) 52 | except CvBridgeError as e: 53 | print(e) 54 | 55 | #--------------- MAIN LOOP 56 | def main(args): 57 | #--- Create the object from the class we defined before 58 | ic = image_converter() 59 | 60 | #--- Initialize the ROS node 61 | rospy.init_node('image_converter', anonymous=True) 62 | try: 63 | rospy.spin() 64 | except KeyboardInterrupt: 65 | print("Shutting down") 66 | 67 | #--- In the end remember to close all cv windows 68 | cv2.destroyAllWindows() 69 | 70 | if __name__ == '__main__': 71 | main(sys.argv) 72 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | pages: 2 | stage: deploy 3 | script: 4 | - mkdir .public 5 | - cp -r doc/html/* .public/ 6 | - mv .public public 7 | artifacts: 8 | paths: 9 | - public 10 | only: 11 | - master 12 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(i2cpwm_board) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp std_msgs message_generation) 5 | 6 | 7 | add_message_files(DIRECTORY msg FILES Servo.msg ServoArray.msg ServoConfig.msg ServoConfigArray.msg Position.msg PositionArray.msg) 8 | 9 | add_service_files(DIRECTORY srv FILES IntValue.srv ServosConfig.srv DriveMode.srv StopServos.srv) 10 | 11 | generate_messages(DEPENDENCIES std_msgs) 12 | 13 | 14 | catkin_package(CATKIN_DEPENDS roscpp std_msgs message_runtime) 15 | 16 | 17 | include_directories(include ${catkin_INCLUDE_DIRS}) 18 | link_directories(${catkin_LIBRARY_DIRS}) 19 | 20 | 21 | add_executable(i2cpwm_board src/i2cpwm_controller.cpp) 22 | target_link_libraries(i2cpwm_board ${catkin_LIBRARIES}) 23 | add_dependencies(i2cpwm_board i2cpwm_board_generate_messages_cpp) 24 | 25 | install(TARGETS i2cpwm_board 26 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 27 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 28 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 29 | ) 30 | install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" ) 31 | install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch ) 32 | 33 | execute_process(COMMAND doxygen doc/Doxyfile) 34 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/README.md: -------------------------------------------------------------------------------- 1 | ros-i2cpwmboard is the project for the i2cpwm_board controller node. 2 | 3 | The user documentation is available at: http://bradanlane.gitlab.io/ros-i2cpwmboard/ 4 | 5 | Integrated demo video: https://vimeo.com/193201509 6 | LoCoRo Robot demo video: https://vimeo.com/189997166 7 | 8 | Status: parameter initialization added. 9 | 10 | The LoCoRo (low cost robot) project configuration and launch files have been moved to a separate poroject: https://gitlab.com/bradanlane/locoro 11 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/footer.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 12 | 13 | 14 |
15 | $generatedby   16 | doxygen 17 | $doxygenversion 18 |
19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/header.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | $projectname: $title 9 | $title 10 | 11 | 12 | 13 | $treeview 14 | $search 15 | $mathjax 16 | 17 | $extrastylesheet 18 | 19 | 20 |
21 | 22 | 23 |
24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 37 | 38 | 39 | 40 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 |
32 |
$projectname 33 |  $projectnumber 34 |
35 |
$projectbrief
36 | 		41 |
$projectbrief
42 | 		$searchbox
53 |
54 | 55 | 56 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/arrowdown.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/arrowdown.png -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/arrowright.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/arrowright.png -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/bc_s.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/bc_s.png -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/bdwn.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/bdwn.png -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/closed.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/closed.png -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/dir_68267d1309a1af8e8297ef4c3efbcdba.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | i2cpwm_board: src Directory Reference 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 |
17 |
18 | 19 | 20 | 21 | 27 | 28 | 29 |
22 |
i2cpwm_board 23 |  0.5.1 24 |
25 |
ROS Package for PCA9685 based 16 channel PWM Board with I2C Interface
26 |
30 |
31 | 32 | 33 | 37 |
38 |
39 |
40 |
src Directory Reference
41 |
42 |
43 | 44 | 46 | 47 | 48 | 49 |

45 | Files

file  i2cpwm_controller.cpp [code]
 controller for I2C interfaced 16 channel PWM boards with PCA9685 chip
 
50 |
51 | 52 | 53 |
54 | Generated by   55 | doxygen 56 | 1.8.11 57 |
58 | 59 | 60 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/doc.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/doc.png -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/doxygen.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/doxygen.png -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/files.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | i2cpwm_board: File List 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 |
17 |
18 | 19 | 20 | 21 | 27 | 28 | 29 |
22 |
i2cpwm_board 23 |  0.5.1 24 |
25 |
ROS Package for PCA9685 based 16 channel PWM Board with I2C Interface
26 |
30 |
31 | 32 | 33 |
34 |
35 |
36 |
File List
37 |
38 |
39 |
Here is a list of all documented files with brief descriptions:
40 |
[detail level 12]
41 | 42 | 43 |
 ▼ src
 i2cpwm_controller.cppController for I2C interfaced 16 channel PWM boards with PCA9685 chip
44 |
45 |
46 | 47 | 48 |
49 | Generated by   50 | doxygen 51 | 1.8.11 52 |
53 | 54 | 55 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/folderclosed.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/folderclosed.png -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/folderopen.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/folderopen.png -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/globals.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | i2cpwm_board: Globals 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 |
17 |
18 | 19 | 20 | 21 | 27 | 28 | 29 |
22 |
i2cpwm_board 23 |  0.5.1 24 |
25 |
ROS Package for PCA9685 based 16 channel PWM Board with I2C Interface
26 |
30 |
31 | 32 | 33 |
34 |
35 |
Here is a list of all documented functions, variables, defines, enums, and typedefs with links to the documentation:
58 |
59 | 60 | 61 |
62 | Generated by   63 | doxygen 64 | 1.8.11 65 |
66 | 67 | 68 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/globals_func.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | i2cpwm_board: Globals 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 |
17 |
18 | 19 | 20 | 21 | 27 | 28 | 29 |
22 |
i2cpwm_board 23 |  0.5.1 24 |
25 |
ROS Package for PCA9685 based 16 channel PWM Board with I2C Interface
26 |
30 |
31 | 32 | 33 |
34 |
35 |   58 |
59 | 60 | 61 |
62 | Generated by   63 | doxygen 64 | 1.8.11 65 |
66 | 67 | 68 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/modules.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | i2cpwm_board: Modules 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 |
17 |
18 | 19 | 20 | 21 | 27 | 28 | 29 |
22 |
i2cpwm_board 23 |  0.5.1 24 |
25 |
ROS Package for PCA9685 based 16 channel PWM Board with I2C Interface
26 |
30 |
31 | 32 | 33 |
34 |
35 |
36 |
Modules
37 |
38 |
39 |
Here is a list of all modules:
40 | 41 | 42 | 43 |
 Topics with subscribers provided by this package
 Services interfaces provided by this package
44 |
45 |
46 | 47 | 48 |
49 | Generated by   50 | doxygen 51 | 1.8.11 52 |
53 | 54 | 55 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/nav_f.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/nav_f.png -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/nav_g.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/nav_g.png -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/nav_h.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/nav_h.png -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/open.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/open.png -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/splitbar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/splitbar.png -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/styles.css: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/styles.css -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/sync_off.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/sync_off.png -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/sync_on.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/sync_on.png -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/tab_a.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/tab_a.png -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/tab_b.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/tab_b.png -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/tab_h.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/tab_h.png -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/tab_s.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/html/tab_s.png -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/html/tabs.css: -------------------------------------------------------------------------------- 1 | .tabs, .tabs2, .tabs3 { 2 | background-image: url('tab_b.png'); 3 | width: 100%; 4 | z-index: 101; 5 | font-size: 13px; 6 | font-family: 'Lucida Grande',Geneva,Helvetica,Arial,sans-serif; 7 | } 8 | 9 | .tabs2 { 10 | font-size: 10px; 11 | } 12 | .tabs3 { 13 | font-size: 9px; 14 | } 15 | 16 | .tablist { 17 | margin: 0; 18 | padding: 0; 19 | display: table; 20 | } 21 | 22 | .tablist li { 23 | float: left; 24 | display: table-cell; 25 | background-image: url('tab_b.png'); 26 | line-height: 36px; 27 | list-style: none; 28 | } 29 | 30 | .tablist a { 31 | display: block; 32 | padding: 0 20px; 33 | font-weight: bold; 34 | background-image:url('tab_s.png'); 35 | background-repeat:no-repeat; 36 | background-position:right; 37 | color: #283A5D; 38 | text-shadow: 0px 1px 1px rgba(255, 255, 255, 0.9); 39 | text-decoration: none; 40 | outline: none; 41 | } 42 | 43 | .tabs3 .tablist a { 44 | padding: 0 10px; 45 | } 46 | 47 | .tablist a:hover { 48 | background-image: url('tab_h.png'); 49 | background-repeat:repeat-x; 50 | color: #fff; 51 | text-shadow: 0px 1px 1px rgba(0, 0, 0, 1.0); 52 | text-decoration: none; 53 | } 54 | 55 | .tablist li.current a { 56 | background-image: url('tab_a.png'); 57 | background-repeat:repeat-x; 58 | color: #fff; 59 | text-shadow: 0px 1px 1px rgba(0, 0, 0, 1.0); 60 | } 61 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/doc/styles.css: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tizianofiorenzani/ros_tutorials/27087d101ce9779924ae7ca0f097a01978e12fbb/ros-i2cpwmboard/doc/styles.css -------------------------------------------------------------------------------- /ros-i2cpwmboard/launch/i2cpwm_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/msg/Position.msg: -------------------------------------------------------------------------------- 1 | # the position message is used when configuring drive mode to 2 | # assign a servo to a specific wheel positon in the drive system 3 | # postions are specific toe the desired drive mode 4 | # ackerman has only one position 5 | # 1 = drive 6 | # differential has two positons 7 | # 1 = left, 2 = right 8 | # mecanum has four positions 9 | # 1 = front left, 2 = front right, 3 = rear left, 4 = rear right 10 | # multiple servos/motors may be used for each positon 11 | 12 | int16 servo 13 | int16 position 14 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/msg/PositionArray.msg: -------------------------------------------------------------------------------- 1 | # the PositionArray message handles multiple position assignments 2 | # of servos. 3 | 4 | Position[] servos 5 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/msg/Servo.msg: -------------------------------------------------------------------------------- 1 | # the Servo message is commonly used message in this package to 2 | # assign a value to a specific servo. the purpose of the value is 3 | # dependent on the topic or service being called 4 | 5 | int16 servo 6 | float32 value 7 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/msg/ServoArray.msg: -------------------------------------------------------------------------------- 1 | # the ServoArray message is commonly used message in this package to 2 | # handle multiple assignments of values to servos. the purpose of 3 | # the value is dependent on the topic or service being called 4 | 5 | Servo[] servos 6 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/msg/ServoConfig.msg: -------------------------------------------------------------------------------- 1 | # the ServoConfig message is used to assign specific configuration 2 | # data to a servo. the data is needed to normalize servos to 3 | # common values to isolate variations from the rest of the user's 4 | # robot motion code. 5 | 6 | int16 servo 7 | int16 center 8 | int16 range 9 | int16 direction 10 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/msg/ServoConfigArray.msg: -------------------------------------------------------------------------------- 1 | # the ServoConfigArray message is used to handle multiple assignments 2 | # of config data to servos. the data is needed to normalize servos to 3 | # common values to isolate variations from the rest of the user's 4 | # robot motion code. 5 | 6 | ServoConfig[] servos 7 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | i2cpwm_board 4 | 0.5.1 5 | 6 | Controller for the Adafruit 16 channel PWM servo I2C board, hat, and similar PCA9685 based boards. 7 | Provides absolute proportional PWM control as well as supports geometry_msgs/Twist. 8 | 9 | 10 | Bradan Lane Studio 11 | bradanlane 12 | GPLv3 13 | https://gitlab.com/bradanlane/ros-i2cpwmboard 14 | 15 | catkin 16 | 17 | message_generation 18 | std_msgs 19 | std_srvs 20 | roscpp 21 | rospy 22 | 23 | message_runtime 24 | std_msgs 25 | std_srvs 26 | roscpp 27 | rospy 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/srv/DriveMode.srv: -------------------------------------------------------------------------------- 1 | # the drive_mode service is used to assigned servos to various drive modes 2 | # the drive modes determine how the assigned servos respond to geometry_msgs::Twist messages 3 | # drive modes are one of: ackerman, differential, or mecanum 4 | # to accurately convert velocity in m/s the controller needs to know three values: 5 | # the RPM - the maximum output speed available from the drive motors 6 | # the radius - the drive wheel radius in meters 7 | # the track - the distance between the left and right wheels in meters 8 | # use the scale value to adjust incoming Twist values as needed to match the servo/motor capability 9 | 10 | 11 | string mode 12 | float32 rpm 13 | float32 radius 14 | float32 track 15 | float32 scale 16 | Position[] servos 17 | --- 18 | int16 error 19 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/srv/IntValue.srv: -------------------------------------------------------------------------------- 1 | # there are a few services whic take a single integer as input 2 | # these services share the IntValue service definition 3 | 4 | int16 value 5 | --- 6 | int16 error 7 | 8 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/srv/ServosConfig.srv: -------------------------------------------------------------------------------- 1 | # the servos_config service is used to assign useful configuration data to servos 2 | # the tollerance of electronis varies in RC servos so it is important to calibate 3 | # each servo, indicating its PWM value for direction of rotation, centering, and 4 | # range which is used to scale servo motion a standard ±1000 scale 5 | 6 | ServoConfig[] servos 7 | --- 8 | int16 error 9 | -------------------------------------------------------------------------------- /ros-i2cpwmboard/srv/StopServos.srv: -------------------------------------------------------------------------------- 1 | 2 | 3 | --- 4 | -------------------------------------------------------------------------------- /test_pub_sub/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(test_pub_sub) 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 | roscpp 12 | rospy 13 | message_generation 14 | ) 15 | 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 exec_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 exec_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 | test_custom_msg.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 exec_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 test_pub_sub 108 | CATKIN_DEPENDS roscpp rospy message_runtime 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}/test_pub_sub.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/test_pub_sub_node.cpp) 137 | 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 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | # target_link_libraries(${PROJECT_NAME}_node 150 | # ${catkin_LIBRARIES} 151 | # ) 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_test_pub_sub.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 | -------------------------------------------------------------------------------- /test_pub_sub/msg/test_custom_msg.msg: -------------------------------------------------------------------------------- 1 | string data 2 | int32 counter 3 | -------------------------------------------------------------------------------- /test_pub_sub/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | test_pub_sub 4 | 0.0.0 5 | The test_pub_sub package 6 | 7 | 8 | 9 | 10 | tiffo 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 | message_generation 41 | 42 | 43 | 44 | 45 | 46 | message_runtime 47 | message_runtime 48 | 49 | 50 | 51 | 52 | catkin 53 | roscpp 54 | rospy 55 | roscpp 56 | rospy 57 | roscpp 58 | rospy 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /test_pub_sub/src/test_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from std_msgs.msg import String 5 | 6 | def publisher(): 7 | 8 | pub = rospy.Publisher('string_publish', String, queue_size=10) 9 | 10 | rate = rospy.Rate(1) 11 | 12 | msg_to_publish = String() 13 | 14 | counter = 0 15 | 16 | while not rospy.is_shutdown(): 17 | string_to_publish = "Publishing %d"%counter 18 | counter += 1 19 | 20 | msg_to_publish.data = string_to_publish 21 | pub.publish(msg_to_publish) 22 | 23 | rospy.loginfo(string_to_publish) 24 | 25 | rate.sleep() 26 | 27 | 28 | if __name__ == "__main__": 29 | rospy.init_node("simple_publisher") 30 | publisher() 31 | -------------------------------------------------------------------------------- /test_pub_sub/src/test_publisher_custom.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from test_pub_sub.msg import test_custom_msg 5 | 6 | def publisher(): 7 | 8 | pub = rospy.Publisher('string_publish', test_custom_msg, queue_size=10) 9 | 10 | rate = rospy.Rate(1) 11 | 12 | msg_to_publish = test_custom_msg() 13 | 14 | counter = 0 15 | 16 | while not rospy.is_shutdown(): 17 | string_to_publish = "Publishing %d"%counter 18 | counter += 1 19 | 20 | msg_to_publish.data = string_to_publish 21 | msg_to_publish.counter = counter 22 | pub.publish(msg_to_publish) 23 | 24 | rospy.loginfo(string_to_publish) 25 | 26 | rate.sleep() 27 | 28 | 29 | if __name__ == "__main__": 30 | rospy.init_node("simple_publisher") 31 | publisher() 32 | -------------------------------------------------------------------------------- /test_pub_sub/src/test_subscriber.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from std_msgs.msg import String 5 | 6 | def subscriber(): 7 | sub = rospy.Subscriber('string_publish_2', String, callback_function) 8 | 9 | rospy.spin() 10 | 11 | def callback_function(message): 12 | rospy.loginfo("I received: %s"%message.data) 13 | 14 | if __name__ == "__main__": 15 | rospy.init_node("simple_subscriber") 16 | subscriber() 17 | -------------------------------------------------------------------------------- /test_pub_sub/src/test_subscriber_custom.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from test_pub_sub.msg import test_custom_msg 5 | 6 | def subscriber(): 7 | sub = rospy.Subscriber('string_publish', test_custom_msg, callback_function) 8 | 9 | rospy.spin() 10 | 11 | def callback_function(message): 12 | string_received = message.data 13 | counter_received = message.counter 14 | rospy.loginfo("I received: %d"%counter_received) 15 | 16 | if __name__ == "__main__": 17 | rospy.init_node("simple_subscriber") 18 | subscriber() 19 | --------------------------------------------------------------------------------