├── CMakeLists.txt ├── README.md ├── connection ├── CMakeLists.txt ├── package.xml └── scripts │ ├── .idea │ ├── misc.xml │ ├── modules.xml │ ├── scripts.iml │ └── workspace.xml │ ├── husky_sender.py │ ├── husky_sender_new.py │ ├── server.py │ └── vo_sender.py ├── demo_launch ├── CMakeLists.txt ├── launch │ └── sbox_demo_launch.launch └── package.xml ├── husky_ws ├── husky_back.tar.gz ├── husky_change_file │ ├── amcl_demo.launch │ └── demodisplay_husky.urdf.xacro ├── maps │ ├── cartographer_demo │ │ ├── map.pgm │ │ └── map.yaml │ ├── demo_map │ │ ├── map.pgm │ │ └── map.yaml │ ├── map.pgm │ └── map.yaml ├── pose │ ├── cartographer_demo │ │ └── pose.txt │ ├── demo_pose │ │ ├── husky_demo_get_pose.py │ │ └── pose.txt │ ├── husky_demo_get_pose.py │ └── pose.txt └── test │ ├── map.pgm │ ├── map.yaml │ └── pose.txt ├── pose_tf ├── CMakeLists.txt ├── package.xml └── scripts │ ├── manual-wp-2505.txt │ ├── pose_loader.py │ ├── talker.py │ ├── waypoints-old.txt │ └── waypoints.txt ├── vis_marker ├── CMakeLists.txt ├── package.xml ├── script │ └── show_err_msg.py └── src │ ├── err_mark_show.cpp │ ├── fake_path_show.cpp │ ├── husky_err_calc.cpp │ ├── husky_mark_show.cpp │ ├── record_fake_points.cpp │ └── vo_mark_show.cpp └── vrpn2gmapping ├── CMakeLists.txt ├── package.xml └── src ├── readAndPub.cpp └── record.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/kinetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 2D Autonomous Navigation with SensorBox (Velodyne + Husky Navigation Stack) 2 | 3 | I know that with such an amazing set of sensors, as is SensorBox, performing 2D navigation with the traditional Husky navigation stack is definetly not mind blowing. But as we had problems in figuring out the initial best strategy to achieve a robust 2D navigation with SensorBox, I want to write down my learnings on this post in order to help others who are facing my same difficulties. 4 | 5 | # SensorBox 6 | 7 | SensorBox is a set of sensors binded togheter in Shanghai Jiao Tong University and BeiDou Research Institute that aims at performing indoor autonomous navigation by fusing the information retrieved by each sensor (sensor fusion). 8 | SesorBox is composed of: 9 | - 1 embedded computer (Ubuntu 16.04 ROS Kinetic) 10 | - 1 Velodyne 3D LiDAR 11 | - 2 cameras 12 | - 1 IMU 13 | 14 | In this post we will focus on the traditional 2D navigation and we will hack Velodyne in order to make it working as it was a traditional 2D laser scanner (e.g RPLidar A3, Hokuyo etc). 15 | 16 | [![IMAGE ALT TEXT HERE](https://img.youtube.com/vi/JbJeOqDqstU/0.jpg)](https://www.youtube.com/watch?v=JbJeOqDqstU) 17 | 18 | # STEPS 19 | Assuming we have ROS up and running, first we need to install all the Husky navigation stack on our embedded computer. While installing the ROS packages is quite straightforward 20 | 21 | ``` sudo apt-get install ros-kinetic-husky-navigation ros-kinetic-husky-bringup ros-kinetic-husky-control ros-kinetic-husky-viz ``` 22 | 23 | we need to install also the husky-core service as [in this post in the wiki ROS](http://wiki.ros.org/husky_bringup/Tutorials/Install%20Husky%20Software%20%28Advanced%29) and the catch is that obviously you don't need to install all the Husky Ubuntu image from scratch but, as in the quoted tutorial, by creating and configuring properly the robot-wide setup file the husky-core service will work as a charm. 24 | 25 | Now the trick is to transform the 3D Lidar Velodyne in a normal 2D laser scanner - I know a big waste of data... but it works!! So as in [this post](https://github.com/MengGuo/Jackal_Velodyne_Duke/tree/master/navigation) we install ``` pointcloud_to_laserscan ``` package from this [repo](https://github.com/ros-perception/pointcloud_to_laserscan) and same as in Jackal_Velodyne_Duke github post we edit the launch file "sample_node.launch" 26 | 27 | ``` 28 | 29 | 30 | 31 | 32 | 33 | 34 | transform_tolerance: 0.01 35 | min_height: 0.25 36 | max_height: 0.75 37 | 38 | angle_min: -3.1415 39 | angle_max: 3.1415 40 | angle_increment: 0.01 41 | scan_time: 0.1 42 | range_min: 0.9 43 | range_max: 130 44 | use_inf: true 45 | concurrency_level: 0 46 | 47 | 48 | 49 | 50 | 51 | ``` 52 | 53 | while obviously adjusting the ROS topics (for me the topic /scan works perfectly on Rviz). 54 | 55 | Finally we adjust the transformation from Velodyne to BaseLink by adding a static transformation to our "sample_node.launch" file 56 | 57 | ``` ``` 58 | 59 | and we are good to go! 60 | 61 | # RUN THE CODE 62 | 63 | When you start the robot make sure: 64 | - the ethernet connection is established with Velodyne (No Internet connection) 65 | - plug in the USB HUB in order to connect SensorBox to Husky and the Bluetooth dongle 66 | - unplug all other USB such as WIFI dongle etc (I have noticed that otherwise the computer becomes really slow) 67 | 68 | Making the map with Gmapping: 69 | 70 | ``` sudo service husky_core stop ``` 71 | 72 | ``` sudo service husky_core start``` 73 | (it's good to restart Husky drivers) 74 | 75 | ``` roslaunch velodyne_pointcloud VLP16_points.launch ``` 76 | 77 | ``` roslaunch pointcloud_to_laserscan sample_node.launch ``` 78 | 79 | ``` roslaunch husky_navigation gmapping_demo.launch ``` 80 | 81 | ``` roslaunch husky_viz view_robot.launch ``` 82 | 83 | 84 | Using AMCL, I have created a launch file to make it easier: 85 | 86 | ``` roslaunch demo_launch sbox_demo_launch.launch ``` 87 | 88 | Have fun! 89 | -------------------------------------------------------------------------------- /connection/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(connection) 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 | std_msgs 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 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs 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 connection 108 | # CATKIN_DEPENDS roscpp rospy std_msgs 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}/connection.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/connection_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_connection.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 | -------------------------------------------------------------------------------- /connection/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | connection 4 | 0.0.0 5 | The connection package 6 | 7 | 8 | 9 | 10 | wang 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 | roscpp 53 | rospy 54 | std_msgs 55 | roscpp 56 | rospy 57 | std_msgs 58 | roscpp 59 | rospy 60 | std_msgs 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /connection/scripts/.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /connection/scripts/.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /connection/scripts/.idea/scripts.iml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | -------------------------------------------------------------------------------- /connection/scripts/.idea/workspace.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 112 | 113 | 114 | 115 | ADDR 116 | username 117 | clients 118 | chat 119 | true 120 | Deal 121 | chatwith 122 | tcpCliSock 123 | test 124 | data 125 | 126 | 127 | 128 | 140 | 141 | 142 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 |