├── LICENSE ├── README.md ├── gazebo_demo ├── CMakeLists.txt ├── joystick_teleop │ └── joystick_controler_new.py ├── launch │ ├── Test.launch │ ├── demo.launch │ ├── empty_world.launch │ ├── gazebo_demo_world.launch │ ├── gazebo_demo_world_maze.launch │ ├── hector_slam.launch │ ├── joystick.launch │ ├── object_map.launch │ ├── object_map_test.launch │ ├── object_yaml.launch │ └── put_robot_in_world.launch ├── package.xml ├── urdf │ ├── ABot.urdf.xacro │ ├── hokuyo.dae │ └── hokuyo.xacro └── worlds │ ├── demo_world.world │ ├── model.config │ ├── model.world │ └── objects │ ├── beer │ ├── materials │ │ ├── scripts │ │ │ └── beer.material │ │ └── textures │ │ │ └── beer.png │ ├── model-1_4.sdf │ ├── model.config │ └── model.sdf │ ├── chair_2 │ ├── materials │ │ ├── scripts │ │ │ ├── chair_2_cushion.material │ │ │ └── chair_2_frame.material │ │ └── textures │ │ │ ├── fabric_red.jpg │ │ │ └── wood_light.png │ ├── meshes │ │ ├── cushion.dae │ │ ├── frame.dae │ │ └── model.dae │ ├── model.config │ └── model.sdf │ ├── dog.dae │ ├── human_male_1 │ ├── materials │ │ ├── scripts │ │ │ ├── hair_m_1_1.material │ │ │ └── skin_m_1.material │ │ └── textures │ │ │ ├── hair_1.jpg │ │ │ ├── hair_2.jpg │ │ │ ├── hair_3.jpg │ │ │ ├── hair_4.jpg │ │ │ ├── skin_1.jpg │ │ │ ├── skin_2.jpg │ │ │ ├── skin_3.jpg │ │ │ └── skin_4.jpg │ ├── meshes │ │ ├── eye_eyebrow.dae │ │ ├── eyeball.dae │ │ ├── eyebrows.dae │ │ ├── hair.dae │ │ ├── human.dae │ │ ├── lenses.dae │ │ ├── model.dae │ │ ├── pants.dae │ │ ├── pupil.dae │ │ ├── shirt.dae │ │ ├── shoes.dae │ │ ├── skeleton.dae │ │ ├── skeleton_no_hair.dae │ │ └── skin.dae │ ├── model.config │ └── model.sdf │ ├── monitor.dae │ ├── monitor_3 │ ├── materials │ │ ├── scripts │ │ │ └── monitor_screen_3.material │ │ └── textures │ │ │ └── prog_screen.png │ ├── meshes │ │ ├── frame.dae │ │ ├── monitor.dae │ │ └── screen1.dae │ ├── model.config │ └── model.sdf │ ├── pot_flower │ ├── meshes │ │ ├── full.dae │ │ ├── pot_base.dae │ │ └── pot_soil.dae │ ├── model.config │ └── model.sdf │ ├── sitting_cat.stl │ ├── sofa_set_3 │ ├── materials │ │ ├── scripts │ │ │ ├── sofa_set_3_cushion.material │ │ │ └── sofa_set_3_frame.material │ │ └── textures │ │ │ ├── bamboo.jpg │ │ │ └── fabric_red_dark.jpg │ ├── meshes │ │ ├── cushion_single.dae │ │ ├── model.dae │ │ ├── sofa_cushions.dae │ │ ├── sofa_frame.dae │ │ └── sofa_full.dae │ ├── model.config │ └── model.sdf │ └── table_dining │ ├── materials │ ├── scripts │ │ └── dining_table.material │ └── textures │ │ ├── wood_black.jpg │ │ ├── wood_black_1.jpg │ │ └── wood_black_3.png │ ├── meshes │ └── table2.dae │ ├── model.config │ └── model.sdf ├── images └── pipelines.png ├── joint_object_localizer ├── CMakeLists.txt ├── config │ ├── correction_factor.yaml │ ├── object.yaml │ └── object_array.yaml ├── msg │ ├── M_Suggested_List.msg │ ├── OG_List.msg │ ├── Object_Geometry.msg │ └── Optional_Theta.msg ├── package.xml └── scripts │ ├── Object_Detection.py │ ├── Object_Detection_komodo.py │ ├── Object_Detection_new.py │ └── geometric_functions.py ├── komodo_demo ├── CMakeLists.txt ├── launch │ ├── demo.launch │ ├── hector_slam.launch │ ├── joystick.launch │ ├── object_map.launch │ └── ssd.launch └── package.xml ├── object_detector_ssd_tf_ros ├── CMakeLists.txt ├── msg │ ├── SSD_Output.msg │ └── SSD_Outputs.msg ├── package.xml ├── setup.py └── ssd │ ├── __init__.py │ ├── model │ ├── labels.txt │ └── ssd_300_vgg.ckpt.zip │ ├── ssd │ ├── __init__.py │ ├── custom_layers.py │ ├── np_methods.py │ ├── ssd_common.py │ ├── ssd_vgg_300.py │ ├── ssd_vgg_preprocessing.py │ ├── tf_image.py │ └── tf_image.pyc │ ├── ssd_node.py │ ├── ssd_wrapper.py │ └── tf_extended │ ├── __init__.py │ ├── bboxes.py │ ├── image.py │ ├── math.py │ ├── metrics.py │ └── tensors.py └── object_mapping ├── CMakeLists.txt ├── config └── theta_cov.yaml ├── msg ├── M.msg ├── M_i.msg ├── Object.msg ├── Object_Map.msg └── Single_Class.msg ├── object_mapper ├── H_functions.py ├── H_functions.pyc ├── Object_Mapper.py ├── Publish_object_to_rviz.py ├── Results_code │ ├── Results_table_excel.py │ ├── TV_xls.py │ ├── dog_xls.py │ ├── making_results.py │ └── map2csv.py ├── Updated_object_mapping.py ├── confusion_matrix_corrected.csv ├── map2rviz.py ├── obj_class.py └── obj_class.pyc └── package.xml /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Or Tslil and Amit Elbaz 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # object_map 2 | By [Or Tslil](https://github.com/ortslil64), [Amit Elbaz](https://github.com/elbazam) 3 | 4 | [Paper link](https://www.researchgate.net/publication/342735084_Representing_and_updating_objects'_identities_in_semantic_SLAM) 5 | 6 | ROS implementation of online semantic SLAM, based on the a published paper - "Representing and updating object identities in semantic SLAM". The object detection node is based on SSD300 architecture and forked from https://github.com/balancap/SSD-Tensorflow. 7 | ## Example - Gazebo simulation 8 | [![Watch the video](https://img.youtube.com/vi/-H25q_Vcol8/default.jpg)](https://youtu.be/-H25q_Vcol8) 9 | ## Example - Experiment 10 | [![Watch the video](https://img.youtube.com/vi/mQHh478gTg8/default.jpg)](https://youtu.be/mQHh478gTg8) 11 | ## Pipelines 12 |

13 | accessibility text 14 |

15 | 16 | 17 | 18 | ## Dependencies 19 | The following python packges are required: 20 | * python 2.* 21 | * numpy 22 | * sklearn 23 | * sciPy 24 | * openCV 25 | * TensorFlow 1.1* (GPU version) 26 | * hector_mapping (http://wiki.ros.org/hector_mapping) 27 | * currently tested in ros melodic in ubuntu 18.04 28 | ## Setup 29 | 1. Download repository to your catkin workspace: 30 | ```bash 31 | git clone https://github.com/or-tal-robotics/object_map.git 32 | ``` 33 | 2. Build: 34 | ```bash 35 | catkin_make 36 | ``` 37 | 3. Install SSD image detector for ROS: 38 | ```bash 39 | pip install -e object_detector_ssd_tf_ros 40 | ``` 41 | 4. Unzip SSD weights in `object_map/object_detector_ssd_tf_ros/ssd/model/ssd_300_vgg.ckpt.zip` 42 | 43 | ## Runing 44 | * For a demo simulation use: 45 | ```bash 46 | roslaunch gazebo_demo demo.launch 47 | ``` 48 | * For a demo simulation working with the "Bhattacharyya coefficient" method of updating the map use: 49 | ```bash 50 | roslaunch gazebo_demo Test.launch 51 | ``` 52 | -------------------------------------------------------------------------------- /gazebo_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(gazebo_demo) 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 | geometry_msgs 12 | message_generation 13 | message_runtime 14 | roscpp 15 | rospy 16 | std_msgs 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | # find_package(Boost REQUIRED COMPONENTS system) 21 | 22 | 23 | ## Uncomment this if the package has a setup.py. This macro ensures 24 | ## modules and global scripts declared therein get installed 25 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 26 | # catkin_python_setup() 27 | 28 | ################################################ 29 | ## Declare ROS messages, services and actions ## 30 | ################################################ 31 | 32 | ## To declare and build messages, services or actions from within this 33 | ## package, follow these steps: 34 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 35 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 36 | ## * In the file package.xml: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 39 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 40 | ## but can be declared for certainty nonetheless: 41 | ## * add a exec_depend tag for "message_runtime" 42 | ## * In this file (CMakeLists.txt): 43 | ## * add "message_generation" and every package in MSG_DEP_SET to 44 | ## find_package(catkin REQUIRED COMPONENTS ...) 45 | ## * add "message_runtime" and every package in MSG_DEP_SET to 46 | ## catkin_package(CATKIN_DEPENDS ...) 47 | ## * uncomment the add_*_files sections below as needed 48 | ## and list every .msg/.srv/.action file to be processed 49 | ## * uncomment the generate_messages entry below 50 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 51 | 52 | ## Generate messages in the 'msg' folder 53 | # add_message_files( 54 | # FILES 55 | # Message1.msg 56 | # Message2.msg 57 | # ) 58 | 59 | ## Generate services in the 'srv' folder 60 | # add_service_files( 61 | # FILES 62 | # Service1.srv 63 | # Service2.srv 64 | # ) 65 | 66 | ## Generate actions in the 'action' folder 67 | # add_action_files( 68 | # FILES 69 | # Action1.action 70 | # Action2.action 71 | # ) 72 | 73 | ## Generate added messages and services with any dependencies listed here 74 | # generate_messages( 75 | # DEPENDENCIES 76 | # geometry_msgs# std_msgs 77 | # ) 78 | 79 | ################################################ 80 | ## Declare ROS dynamic reconfigure parameters ## 81 | ################################################ 82 | 83 | ## To declare and build dynamic reconfigure parameters within this 84 | ## package, follow these steps: 85 | ## * In the file package.xml: 86 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 87 | ## * In this file (CMakeLists.txt): 88 | ## * add "dynamic_reconfigure" to 89 | ## find_package(catkin REQUIRED COMPONENTS ...) 90 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 91 | ## and list every .cfg file to be processed 92 | 93 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 94 | # generate_dynamic_reconfigure_options( 95 | # cfg/DynReconf1.cfg 96 | # cfg/DynReconf2.cfg 97 | # ) 98 | 99 | ################################### 100 | ## catkin specific configuration ## 101 | ################################### 102 | ## The catkin_package macro generates cmake config files for your package 103 | ## Declare things to be passed to dependent projects 104 | ## INCLUDE_DIRS: uncomment this if your package contains header files 105 | ## LIBRARIES: libraries you create in this project that dependent projects also need 106 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 107 | ## DEPENDS: system dependencies of this project that dependent projects also need 108 | catkin_package( 109 | # INCLUDE_DIRS include 110 | # LIBRARIES SLAM 111 | # CATKIN_DEPENDS geometry_msgs message_generation message_runtime roscpp rospy std_msgs 112 | # DEPENDS system_lib 113 | ) 114 | 115 | ########### 116 | ## Build ## 117 | ########### 118 | 119 | ## Specify additional locations of header files 120 | ## Your package locations should be listed before other locations 121 | include_directories( 122 | # include 123 | ${catkin_INCLUDE_DIRS} 124 | ) 125 | 126 | ## Declare a C++ library 127 | # add_library(${PROJECT_NAME} 128 | # src/${PROJECT_NAME}/SLAM.cpp 129 | # ) 130 | 131 | ## Add cmake target dependencies of the library 132 | ## as an example, code may need to be generated before libraries 133 | ## either from message generation or dynamic reconfigure 134 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 135 | 136 | ## Declare a C++ executable 137 | ## With catkin_make all packages are built within a single CMake context 138 | ## The recommended prefix ensures that target names across packages don't collide 139 | # add_executable(${PROJECT_NAME}_node src/SLAM_node.cpp) 140 | 141 | ## Rename C++ executable without prefix 142 | ## The above recommended prefix causes long target names, the following renames the 143 | ## target back to the shorter version for ease of user use 144 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 145 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 146 | 147 | ## Add cmake target dependencies of the executable 148 | ## same as for the library above 149 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 150 | 151 | ## Specify libraries to link a library or executable target against 152 | # target_link_libraries(${PROJECT_NAME}_node 153 | # ${catkin_LIBRARIES} 154 | # ) 155 | 156 | ############# 157 | ## Install ## 158 | ############# 159 | 160 | # all install targets should use catkin DESTINATION variables 161 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 162 | 163 | ## Mark executable scripts (Python etc.) for installation 164 | ## in contrast to setup.py, you can choose the destination 165 | # install(PROGRAMS 166 | # scripts/my_python_script 167 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | # ) 169 | 170 | ## Mark executables and/or libraries for installation 171 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_SLAM.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /gazebo_demo/joystick_teleop/joystick_controler_new.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from geometry_msgs.msg import Twist 5 | from sensor_msgs.msg import Joy 6 | 7 | Move = [0,0,0,0] 8 | 9 | def callback(joy): 10 | global Move 11 | Move = [joy.buttons[4],-joy.buttons[0],joy.buttons[3],-joy.buttons[1]] 12 | #print "pose callback" 13 | 14 | def main (): 15 | global Move 16 | 17 | try: 18 | pub = rospy.Publisher('/cmd_vel', Twist, queue_size = 10) 19 | rospy.Subscriber('/joy', Joy, callback) 20 | rospy.init_node('keyboard', anonymous=True) 21 | 22 | rate = rospy.Rate(90) 23 | 24 | while not rospy.is_shutdown(): 25 | #left = Float64() 26 | #right = Float64() 27 | vel = Twist() 28 | 29 | #left.data = (Move[0]+Move[1])*3 30 | #right.data = (Move[2]+Move[3])*3 31 | #left.data = (Move[0] - Move[3] + Move[1]) * 3 32 | #right.data = (Move[0] + Move[2] + Move[1]) * 3 33 | vel.linear.x = (Move[0] + Move[1]) * 0.5 34 | vel.angular.z = (Move[2] + Move[3])*0.3 35 | pub.publish(vel) 36 | 37 | #pub_left.publish(left) 38 | #pub_right.publish(right) 39 | 40 | 41 | rate.sleep() 42 | #left.data = 0; right.data = 0 43 | #pub_left.publish(left) 44 | #pub_right.publish(right) 45 | rospy.spin() 46 | 47 | except Exception as e: 48 | print(e) 49 | 50 | finally: 51 | #left.data = 0; right.data = 0 52 | #pub_left.publish(left) 53 | #pub_right.publish(right) 54 | vel.linear.x = 0; vel.angular.z = 0 55 | vel.linear.y = 0; vel.linear.z = 0; vel.angular.x = 0; vel.angular.y = 0 56 | pub.publish(vel) 57 | 58 | if __name__=="__main__": 59 | 60 | #try: 61 | main() 62 | #except rospy.ROSInterruptException: 63 | #pass 64 | -------------------------------------------------------------------------------- /gazebo_demo/launch/Test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /gazebo_demo/launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /gazebo_demo/launch/empty_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /gazebo_demo/launch/gazebo_demo_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /gazebo_demo/launch/gazebo_demo_world_maze.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /gazebo_demo/launch/hector_slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /gazebo_demo/launch/joystick.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /gazebo_demo/launch/object_map.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /gazebo_demo/launch/object_map_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /gazebo_demo/launch/object_yaml.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /gazebo_demo/launch/put_robot_in_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /gazebo_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gazebo_demo 4 | 0.0.0 5 | The SLAM package 6 | 7 | 8 | 9 | 10 | lab 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 | geometry_msgs 53 | message_generation 54 | roscpp 55 | rospy 56 | std_msgs 57 | geometry_msgs 58 | roscpp 59 | rospy 60 | std_msgs 61 | geometry_msgs 62 | message_runtime 63 | roscpp 64 | rospy 65 | std_msgs 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | -------------------------------------------------------------------------------- /gazebo_demo/urdf/hokuyo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 0 0 0 0 0 0 37 | 38 | 39 | 40 | 41 | 1 42 | 2.0944 43 | -2.0944 44 | 683 45 | 46 | 47 | 48 | 0.08 49 | 40 50 | 0.01 51 | 52 | 53 | 54 | 55 | 56 | scan 57 | base_laser 58 | 59 | 62 | 63 | 1 64 | 30 65 | true 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | my_map 4 | 1.0 5 | model.sdf 6 | 7 | Tal Feiner 8 | feinertal@gmail.com 9 | 10 | Example map, for the simulation 11 | 12 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/beer/materials/scripts/beer.material: -------------------------------------------------------------------------------- 1 | material Beer/Diffuse 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | texture_unit 8 | { 9 | texture beer.png 10 | filtering anistropic 11 | max_anisotropy 16 12 | } 13 | } 14 | } 15 | } 16 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/beer/materials/textures/beer.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/gazebo_demo/worlds/objects/beer/materials/textures/beer.png -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/beer/model-1_4.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 0 0.115 0 0 0 6 | 7 | 0.390 8 | 9 | 0.00058 10 | 0 11 | 0 12 | 0.00058 13 | 0 14 | 0.00019 15 | 16 | 17 | 18 | 19 | 20 | 0.055000 21 | 0.230000 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 0.055000 30 | 0.230000 31 | 32 | 33 | 34 | 35 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/beer/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Beer 5 | 1.0 6 | model-1_4.sdf 7 | model.sdf 8 | 9 | 10 | Maurice Fallon 11 | 12 | 13 | 14 | Beer 15 | 16 | 17 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/beer/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 0 0.115 0 0 0 6 | 7 | 0.390 8 | 9 | 0.00058 10 | 0 11 | 0 12 | 0.00058 13 | 0 14 | 0.00019 15 | 16 | 17 | 18 | 19 | 20 | 0.055000 21 | 0.230000 22 | 23 | 24 | 25 | 26 | 27 | 28 | 0.055000 29 | 0.230000 30 | 31 | 32 | 33 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/chair_2/materials/scripts/chair_2_cushion.material: -------------------------------------------------------------------------------- 1 | material chair_2_cushion 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | texture_unit 8 | { 9 | texture fabric_red.jpg 10 | scale 1 1 11 | } 12 | } 13 | } 14 | } 15 | 16 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/chair_2/materials/scripts/chair_2_frame.material: -------------------------------------------------------------------------------- 1 | material chair_2_frame 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | texture_unit 8 | { 9 | texture wood_light.png 10 | scale 1 1 11 | } 12 | } 13 | } 14 | } 15 | 16 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/chair_2/materials/textures/fabric_red.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/gazebo_demo/worlds/objects/chair_2/materials/textures/fabric_red.jpg -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/chair_2/materials/textures/wood_light.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/gazebo_demo/worlds/objects/chair_2/materials/textures/wood_light.png -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/chair_2/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | chair_2 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/chair_2/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -0.180452 -0.231905 0 0 -0 0 6 | 7 | 0 0 0 0 -0 0 8 | 9 | 10 | model://chair_2/meshes/frame.dae 11 | 1 1 1 12 | 13 | 14 | 15 | 20 | 21 | 1 22 | 0 23 | 24 | 25 | 0 26 | 10 27 | 0 0 0 0 -0 0 28 | 29 | 30 | model://chair_2/meshes/frame.dae 31 | 1 1 1 32 | 33 | 34 | 35 | 36 | 37 | -0.181896 -0.233028 0 0 -0 0 38 | 39 | 0 0 0 0 -0 0 40 | 41 | 42 | model://chair_2/meshes/cushion.dae 43 | 1 1 1 44 | 45 | 46 | 47 | 52 | 53 | 1 54 | 0 55 | 56 | 57 | 0 58 | 10 59 | 0 0 0 0 -0 0 60 | 61 | 62 | model://chair_2/meshes/cushion.dae 63 | 1 1 1 64 | 65 | 66 | 67 | 68 | 1 69 | 1 70 | 71 | 72 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/human_male_1/materials/scripts/hair_m_1_1.material: -------------------------------------------------------------------------------- 1 | material hair_m_1_1 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | texture_unit 8 | { 9 | texture hair_3.jpg 10 | scale 1 1 11 | } 12 | } 13 | } 14 | } 15 | 16 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/human_male_1/materials/scripts/skin_m_1.material: -------------------------------------------------------------------------------- 1 | material skin_m_1 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | texture_unit 8 | { 9 | texture skin_2.jpg 10 | scale 1 1 11 | } 12 | } 13 | } 14 | } 15 | 16 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/human_male_1/materials/textures/hair_1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/gazebo_demo/worlds/objects/human_male_1/materials/textures/hair_1.jpg -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/human_male_1/materials/textures/hair_2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/gazebo_demo/worlds/objects/human_male_1/materials/textures/hair_2.jpg -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/human_male_1/materials/textures/hair_3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/gazebo_demo/worlds/objects/human_male_1/materials/textures/hair_3.jpg -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/human_male_1/materials/textures/hair_4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/gazebo_demo/worlds/objects/human_male_1/materials/textures/hair_4.jpg -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/human_male_1/materials/textures/skin_1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/gazebo_demo/worlds/objects/human_male_1/materials/textures/skin_1.jpg -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/human_male_1/materials/textures/skin_2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/gazebo_demo/worlds/objects/human_male_1/materials/textures/skin_2.jpg -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/human_male_1/materials/textures/skin_3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/gazebo_demo/worlds/objects/human_male_1/materials/textures/skin_3.jpg -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/human_male_1/materials/textures/skin_4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/gazebo_demo/worlds/objects/human_male_1/materials/textures/skin_4.jpg -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/human_male_1/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | human_male_1 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/human_male_1/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0 0 0 0 -0 0 7 | 8 | 9 | model://human_male_1/meshes/eyeball.dae 10 | 1 1 1 11 | 12 | 13 | 14 | 1 15 | 19 | 1 1 1 1 20 | 0.9 0.9 0.9 1 21 | 0.01 0.01 0.01 1 22 | 0 0 0 1 23 | 24 | 1 25 | 0 26 | 27 | 28 | 0 29 | 10 30 | 0 0 0 0 -0 0 31 | 32 | 33 | model://human_male_1/meshes/eyeball.dae 34 | 1 1 1 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 0 0 0 0 -0 0 44 | 45 | 46 | model://human_male_1/meshes/eyebrows.dae 47 | 1 1 1 48 | 49 | 50 | 51 | 56 | 57 | 1 58 | 0 59 | 60 | 61 | 0 62 | 10 63 | 0 0 0 0 -0 0 64 | 65 | 66 | model://human_male_1/meshes/eyebrows.dae 67 | 1 1 1 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 0 0 0 0 -0 0 77 | 78 | 79 | model://human_male_1/meshes/hair.dae 80 | 1 1 1 81 | 82 | 83 | 84 | 89 | 90 | 1 91 | 0 92 | 93 | 94 | 0 95 | 10 96 | 0 0 0 0 -0 0 97 | 98 | 99 | model://human_male_1/meshes/hair.dae 100 | 1 1 1 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 0 0 0 0 -0 0 110 | 111 | 112 | model://human_male_1/meshes/lenses.dae 113 | 1 1 1 114 | 115 | 116 | 117 | 1 118 | 122 | 0.4078 0.2431 0.0471 1 123 | 0.4078 0.2431 0.0471 1 124 | 0.01 0.01 0.01 1 125 | 0 0 0 1 126 | 127 | 1 128 | 0 129 | 130 | 131 | 0 132 | 10 133 | 0 0 0 0 -0 0 134 | 135 | 136 | model://human_male_1/meshes/lenses.dae 137 | 1 1 1 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 0 0 0 0 -0 0 147 | 148 | 149 | model://human_male_1/meshes/pants.dae 150 | 1 1 1 151 | 152 | 153 | 154 | 1 155 | 159 | 0.3529 0.3451 0.3294 1 160 | 0.3529 0.3451 0.3294 1 161 | 0.01 0.01 0.01 1 162 | 0 0 0 1 163 | 164 | 1 165 | 0 166 | 167 | 168 | 0 169 | 10 170 | 0 0 0 0 -0 0 171 | 172 | 173 | model://human_male_1/meshes/pants.dae 174 | 1 1 1 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 0 0 0 0 -0 0 184 | 185 | 186 | model://human_male_1/meshes/pupil.dae 187 | 1 1 1 188 | 189 | 190 | 191 | 1 192 | 0 0 0 1 193 | 0 0 0 1 194 | 0.01 0.01 0.01 1 195 | 0 0 0 1 196 | 197 | 1 198 | 0 199 | 200 | 201 | 0 202 | 10 203 | 0 0 0 0 -0 0 204 | 205 | 206 | model://human_male_1/meshes/pupil.dae 207 | 1 1 1 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 0 0 0 0 -0 0 217 | 218 | 219 | model://human_male_1/meshes/shirt.dae 220 | 1 1 1 221 | 222 | 223 | 224 | 1 225 | 0.8706 0.8471 0.1608 1 226 | 0.8706 0.8471 0.1608 1 227 | 0.01 0.01 0.01 1 228 | 0 0 0 1 229 | 230 | 1 231 | 0 232 | 233 | 234 | 0 235 | 10 236 | 0 0 0 0 -0 0 237 | 238 | 239 | model://human_male_1/meshes/shirt.dae 240 | 1 1 1 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 0 0 0 0 -0 0 249 | 250 | 251 | model://human_male_1/meshes/shoes.dae 252 | 1 1 1 253 | 254 | 255 | 256 | 1 257 | 0.2000 0.1451 0.1451 1 258 | 0.2000 0.1451 0.1451 1 259 | 0.01 0.01 0.01 1 260 | 0 0 0 1 261 | 262 | 1 263 | 0 264 | 265 | 266 | 0 267 | 10 268 | 0 0 0 0 -0 0 269 | 270 | 271 | model://human_male_1/meshes/shoes.dae 272 | 1 1 1 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 0 0 0 0 -0 0 282 | 283 | 284 | model://human_male_1/meshes/skin.dae 285 | 1 1 1 286 | 287 | 288 | 289 | 294 | 295 | 1 296 | 0 297 | 298 | 299 | 0 300 | 10 301 | 0 0 0 0 -0 0 302 | 303 | 304 | model://human_male_1/meshes/skin.dae 305 | 1 1 1 306 | 307 | 308 | 309 | 310 | 311 | 1 312 | 1 313 | 314 | 315 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/monitor_3/materials/scripts/monitor_screen_3.material: -------------------------------------------------------------------------------- 1 | material monitor_screen_3 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | texture_unit 8 | { 9 | texture prog_screen.png 10 | scale 1 1 11 | } 12 | } 13 | } 14 | } 15 | 16 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/monitor_3/materials/textures/prog_screen.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/gazebo_demo/worlds/objects/monitor_3/materials/textures/prog_screen.png -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/monitor_3/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | monitor_3 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/monitor_3/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -0.305832 -0.059311 0 0 -0 0 6 | 7 | 0 0 0 0 -0 0 8 | 9 | 10 | model://monitor_3/meshes/frame.dae 11 | 1 1 1 12 | 13 | 14 | 15 | 1 16 | 0 0 0 1 17 | 0.2 0.2 0.2 1 18 | 0.01 0.01 0.01 1 19 | 0 0 0 1 20 | 21 | __default__ 22 | 23 | 24 | 1 25 | 0 26 | 27 | 28 | 0 29 | 10 30 | 0 0 0 0 -0 0 31 | 32 | 33 | model://monitor_3/meshes/frame.dae 34 | 1 1 1 35 | 36 | 37 | 38 | 39 | 40 | -0.307748 -0.058272 0 0 -0 0 41 | 42 | 0 0 0 0 -0 0 43 | 44 | 45 | model://monitor_3/meshes/screen1.dae 46 | 1 1 1 47 | 48 | 49 | 59 | 60 | 65 | 66 | 1 67 | 0 68 | 69 | 70 | 0 71 | 10 72 | 0 0 0 0 -0 0 73 | 74 | 75 | model://monitor_3/meshes/screen1.dae 76 | 1 1 1 77 | 78 | 79 | 80 | 81 | 1 82 | 1 83 | 84 | 85 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/pot_flower/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | pot_flower 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/pot_flower/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 0 0 0 -0 0 6 | 7 | 1 8 | 0 9 | 0 10 | 11 | 0 0 0 0 -0 0 12 | 13 | 14 | model://pot_flower/meshes/pot_base.dae 15 | 1 1 1 16 | 17 | 18 | 19 | 1 20 | 21 | 0.8156 0.6470 0.1607 1 22 | 0.8156 0.6470 0.1607 1 23 | 0.01 0.01 0.01 1 24 | 0 0 0 1 25 | 26 | __default__ 27 | 28 | 29 | 1 30 | 0 31 | 32 | 33 | 0 34 | 10 35 | 0 0 0 0 -0 0 36 | 37 | 38 | model://pot_flower/meshes/pot_base.dae 39 | 1 1 1 40 | 41 | 42 | 43 | 44 | 45 | 0 0 0 0 -0 0 46 | 47 | 0 0 0 0 -0 0 48 | 49 | 50 | model://pot_flower/meshes/pot_soil.dae 51 | 1 1 1 52 | 53 | 54 | 55 | 1 56 | 57 | 0.4666 0.4196 0.274509804 1 58 | 0.4666 0.4196 0.274509804 1 59 | 0.01 0.01 0.01 1 60 | 0 0 0 1 61 | 62 | __default__ 63 | 64 | 65 | 1 66 | 0 67 | 68 | 69 | 0 70 | 10 71 | 0 0 0 0 -0 0 72 | 73 | 74 | model://pot_flower/meshes/pot_soil.dae 75 | 1 1 1 76 | 77 | 78 | 79 | 80 | 1 81 | 1 82 | 83 | 84 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/sitting_cat.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/gazebo_demo/worlds/objects/sitting_cat.stl -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/sofa_set_3/materials/scripts/sofa_set_3_cushion.material: -------------------------------------------------------------------------------- 1 | material sofa_set_3_cushion 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | texture_unit 8 | { 9 | texture fabric_red_dark.jpg 10 | scale 1 1 11 | } 12 | } 13 | } 14 | } 15 | 16 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/sofa_set_3/materials/scripts/sofa_set_3_frame.material: -------------------------------------------------------------------------------- 1 | material sofa_set_3_frame 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | texture_unit 8 | { 9 | texture bamboo.jpg 10 | scale 1 1 11 | } 12 | } 13 | } 14 | } 15 | 16 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/sofa_set_3/materials/textures/bamboo.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/gazebo_demo/worlds/objects/sofa_set_3/materials/textures/bamboo.jpg -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/sofa_set_3/materials/textures/fabric_red_dark.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/gazebo_demo/worlds/objects/sofa_set_3/materials/textures/fabric_red_dark.jpg -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/sofa_set_3/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | sofa_set_3 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/sofa_set_3/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0.00955 -0.210215 -0.030301 0 -0 0 6 | 7 | 0 0 0 0 -0 0 8 | 9 | 10 | model://sofa_set_3/meshes/sofa_frame.dae 11 | 1 1 1 12 | 13 | 14 | 15 | 20 | 21 | 1 22 | 0 23 | 24 | 25 | 0 26 | 10 27 | 0 0 0 0 -0 0 28 | 29 | 30 | model://sofa_set_3/meshes/sofa_frame.dae 31 | 1 1 1 32 | 33 | 34 | 35 | 36 | 37 | 0.011584 -0.187089 -0.030301 0 -0 0 38 | 39 | 0 0 0 0 -0 0 40 | 41 | 42 | model://sofa_set_3/meshes/cushion_single.dae 43 | 1 1 1 44 | 45 | 46 | 47 | 52 | 53 | 1 54 | 0 55 | 56 | 57 | 0 58 | 10 59 | 0 0 0 0 -0 0 60 | 61 | 62 | model://sofa_set_3/meshes/cushion_single.dae 63 | 1 1 1 64 | 65 | 66 | 67 | 68 | 69 | -0.591256 -0.187699 -0.030301 0 -0 0 70 | 71 | 0 0 0 0 -0 0 72 | 73 | 74 | model://sofa_set_3/meshes/cushion_single.dae 75 | 1 1 1 76 | 77 | 78 | 79 | 84 | 85 | 1 86 | 0 87 | 88 | 89 | 0 90 | 10 91 | 0 0 0 0 -0 0 92 | 93 | 94 | model://sofa_set_3/meshes/cushion_single.dae 95 | 1 1 1 96 | 97 | 98 | 99 | 100 | 101 | -1.201218 -0.187536 -0.030301 0 -0 0 102 | 103 | 0 0 0 0 -0 0 104 | 105 | 106 | model://sofa_set_3/meshes/cushion_single.dae 107 | 1 1 1 108 | 109 | 110 | 111 | 116 | 117 | 1 118 | 0 119 | 120 | 121 | 0 122 | 10 123 | 0 0 0 0 -0 0 124 | 125 | 126 | model://sofa_set_3/meshes/cushion_single.dae 127 | 1 1 1 128 | 129 | 130 | 131 | 132 | 1 133 | 1 134 | 135 | 136 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/table_dining/materials/scripts/dining_table.material: -------------------------------------------------------------------------------- 1 | material dining_table 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | texture_unit 8 | { 9 | texture wood_black_3.png 10 | scale 0.5 0.5 11 | } 12 | } 13 | } 14 | } 15 | 16 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/table_dining/materials/textures/wood_black.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/gazebo_demo/worlds/objects/table_dining/materials/textures/wood_black.jpg -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/table_dining/materials/textures/wood_black_1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/gazebo_demo/worlds/objects/table_dining/materials/textures/wood_black_1.jpg -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/table_dining/materials/textures/wood_black_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/gazebo_demo/worlds/objects/table_dining/materials/textures/wood_black_3.png -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/table_dining/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | table_dining 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /gazebo_demo/worlds/objects/table_dining/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 0 0 0 -0 0 6 | 7 | 0 0 0 0 -0 0 8 | 9 | 10 | model://table_dining/meshes/table2.dae 11 | 1 1 1 12 | 13 | 14 | 24 | 25 | 30 | 31 | 1 32 | 0 33 | 34 | 35 | 0 36 | 10 37 | 0 0 0 0 -0 0 38 | 39 | 40 | model://table_dining/meshes/table2.dae 41 | 1 1 1 42 | 43 | 44 | 45 | 46 | 1 47 | 1 48 | 49 | 50 | -------------------------------------------------------------------------------- /images/pipelines.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/images/pipelines.png -------------------------------------------------------------------------------- /joint_object_localizer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(joint_object_localizer) 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 | std_msgs 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 | Object_Geometry.msg 53 | OG_List.msg 54 | M_Suggested_List.msg 55 | Optional_Theta.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 | 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 joint_object_localizer 110 | CATKIN_DEPENDS message_runtime rospy 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}/joint_object_localizer.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/joint_object_localizer_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 for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 171 | # install(TARGETS ${PROJECT_NAME}_node 172 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 173 | # ) 174 | 175 | ## Mark libraries for installation 176 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 177 | # install(TARGETS ${PROJECT_NAME} 178 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 179 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 180 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 181 | # ) 182 | 183 | ## Mark cpp header files for installation 184 | # install(DIRECTORY include/${PROJECT_NAME}/ 185 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 186 | # FILES_MATCHING PATTERN "*.h" 187 | # PATTERN ".svn" EXCLUDE 188 | # ) 189 | 190 | ## Mark other files for installation (e.g. launch and bag files, etc.) 191 | # install(FILES 192 | # # myfile1 193 | # # myfile2 194 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 195 | # ) 196 | 197 | ############# 198 | ## Testing ## 199 | ############# 200 | 201 | ## Add gtest based cpp test target and link libraries 202 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_joint_object_localizer.cpp) 203 | # if(TARGET ${PROJECT_NAME}-test) 204 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 205 | # endif() 206 | 207 | ## Add folders to be run by python nosetests 208 | # catkin_add_nosetests(test) 209 | -------------------------------------------------------------------------------- /joint_object_localizer/config/correction_factor.yaml: -------------------------------------------------------------------------------- 1 | # Robot location corrections 2 | Komodo: 3 | Correction: 0.3 4 | Simulation: 5 | Correction: 0.15 -------------------------------------------------------------------------------- /joint_object_localizer/config/object.yaml: -------------------------------------------------------------------------------- 1 | # Objects with their sizes, names, and area of sizes. 2 | o2: #bicycle 3 | r: 0 4 | a: 2 5 | b: 0.02 6 | name: 'bicycle' 7 | bound_a: [1.8,2.2] 8 | bound_b: [0.01,0.05] 9 | cov: 10 | - [0.4472,0] 11 | - [0,0.1] 12 | o3: #bird 13 | r: 0 14 | a: 0.1 15 | b: 0.06 16 | name: 'bird' 17 | bound_a: [0.05,0.15] 18 | bound_b: [0.04,0.08] 19 | cov: 20 | - [0.141,0] 21 | - [0,0.1] 22 | o5: #bottle 23 | r: 0.06 24 | a: 0 25 | b: 0 26 | name: 'bottle' 27 | bound_r: [0.02,0.08] 28 | cov: 0.1224 29 | o8: #cat 30 | r: 0 31 | a: 0.1 32 | b: 0.08 33 | name: 'cat' 34 | bound_a: [0.08,0.25] 35 | bound_b: [0.06,0.11] 36 | cov: 37 | - [0.2,0] 38 | - [0,0.141] 39 | o9: # Chair 40 | r: 0 41 | a: 0.2 42 | b: 0.2 43 | name: 'chair' 44 | bound_a: [0.05,0.3] 45 | bound_b: [0.05,0.3] 46 | cov: 47 | - [0.3535,0] 48 | - [0,0.3535] 49 | o12: #dog 50 | r: 0 51 | a: 0.45 52 | b: 0.07 53 | name: 'dog' 54 | bound_a: [0.15,0.6] 55 | bound_b: [0.05,0.12] 56 | cov: 57 | - [0.2236,0] 58 | - [0,0.1732] 59 | o14: #motorbike 60 | r: 0 61 | a: 2 62 | b: 0.04 63 | name: 'motorbike' 64 | bound_a: [1.8,2.2] 65 | bound_b: [0.02,0.05] 66 | cov: 67 | - [0.2,0] 68 | - [0,0.01] 69 | o15: #person 70 | r: 0 71 | a: 0.21 72 | b: 0.09 73 | name: 'person' 74 | bound_a: [0.15,0.25] 75 | bound_b: [0.08,0.12] 76 | cov: 77 | - [0.2,0] 78 | - [0,0.1732] 79 | o16: #pottedplant 80 | r: 0.15 81 | a: 0 82 | b: 0 83 | name: 'pottedplant' 84 | bound_r: [0.1,0.18] 85 | cov: 0.05 86 | o17: #sheep 87 | r: 0 88 | a: 1.5 89 | b: 0.3 90 | name: 'sheep' 91 | bound_a: [1.2,1.7] 92 | bound_b: [0.25,0.35] 93 | cov: 94 | - [0.4472,0] 95 | - [0,0.2236] 96 | o18: #sofa 97 | r: 0 98 | a: 2 99 | b: 0.8 100 | name: 'sofa' 101 | bound_a: [1.5,3] 102 | bound_b: [0.6,0.85] 103 | cov: 104 | - [0.707,0] 105 | - [0,0.4472] 106 | o20: #tvmonitor 107 | r: 0 108 | a: 0.9 109 | b: 0.04 110 | name: 'tvmonitor' 111 | bound_a: [0.4,1.3] 112 | bound_b: [0.03,0.06] 113 | cov: 114 | - [0.3162,0] 115 | - [0,0.1] 116 | -------------------------------------------------------------------------------- /joint_object_localizer/config/object_array.yaml: -------------------------------------------------------------------------------- 1 | round: [5,16] 2 | rectangle: [2,14,18,20] 3 | elliptical: [3,8,9,12,15,17] 4 | 5 | object_list: [2,3,5,8,9,12,14,15,16,17,18,20] 6 | 7 | -------------------------------------------------------------------------------- /joint_object_localizer/msg/M_Suggested_List.msg: -------------------------------------------------------------------------------- 1 | Optional_Theta[] M_list 2 | -------------------------------------------------------------------------------- /joint_object_localizer/msg/OG_List.msg: -------------------------------------------------------------------------------- 1 | Object_Geometry[] object_list 2 | -------------------------------------------------------------------------------- /joint_object_localizer/msg/Object_Geometry.msg: -------------------------------------------------------------------------------- 1 | float64 x_center 2 | float64 y_center 3 | 4 | float64 r 5 | 6 | float64 a 7 | float64 b 8 | float64 angle 9 | 10 | float64 height_factor 11 | float64 object_height 12 | 13 | int16 cls 14 | float64[] probabilities 15 | 16 | float64 Final_Likelihood 17 | 18 | -------------------------------------------------------------------------------- /joint_object_localizer/msg/Optional_Theta.msg: -------------------------------------------------------------------------------- 1 | Object_Geometry[] object_list 2 | -------------------------------------------------------------------------------- /joint_object_localizer/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | joint_object_localizer 4 | 0.0.0 5 | The joint_object_localizer package 6 | 7 | 8 | 9 | 10 | amit 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 | 48 | 49 | 50 | 51 | catkin 52 | rospy 53 | std_msgs 54 | rospy 55 | std_msgs 56 | rospy 57 | std_msgs 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /joint_object_localizer/scripts/geometric_functions.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Libraries: 4 | import rospy 5 | import numpy as np 6 | from scipy.stats import multivariate_normal as mn 7 | from scipy.optimize import differential_evolution 8 | 9 | # Msgs: 10 | from geometry_msgs.msg import Point32 11 | from sensor_msgs.msg import PointCloud 12 | 13 | global Object_cls_list 14 | # YAMLs: 15 | Object_cls_list = np.array(rospy.get_param('/Array/object_list')) 16 | 17 | # Publisher: 18 | Object_P_pub = rospy.Publisher('/OB_Points' , PointCloud , queue_size = 5 ) 19 | 20 | def Ranges_to_xy_plane(R,angle_min,angle_max, 21 | size,angle_jumps, 22 | max_dist = 2): 23 | 24 | '''Transfer the ranges into (x,y) dots. return array [n,2]''' 25 | # Initializing an inf values: 26 | R[np.isinf(R)] = 0 27 | 28 | # Initializing an inf values: 29 | R[np.isnan(R)] = 0 30 | 31 | # Checking info about the location of the object: 32 | Dist_check = np.array(R[angle_min:angle_max]) 33 | Dist_check = Dist_check[Dist_check != 0] 34 | 35 | # If there are not enough measuremrnts: 36 | if len(Dist_check) < 4: 37 | return np.array([0,0]) 38 | # If object is too far away: 39 | if np.amin(Dist_check) > max_dist: 40 | return np.array([0,0]) 41 | 42 | print ('Close enough, starting.') 43 | 44 | laser_depth_observations = np.zeros((2*size + 1,2)) 45 | P = PointCloud() 46 | P.points = [] 47 | P.header.frame_id = 'laser_link' 48 | for i in range (0,size): 49 | 50 | if R[i] == 0: 51 | laser_depth_observations[i,0] = 100000 52 | laser_depth_observations[i,1] = 100000 53 | continue 54 | laser_depth_observations[i,0] = -R[i] * np.cos((size - i) * angle_jumps - np.pi) 55 | laser_depth_observations[i,1] = R[i] * np.sin((size - i) * angle_jumps - np.pi) 56 | 57 | if i < angle_max and i >= angle_min: 58 | xyz = Point32() 59 | xyz.x = laser_depth_observations[i,0] 60 | xyz.y = laser_depth_observations[i,1] 61 | xyz.z = 0 62 | P.points.append(xyz) 63 | 64 | 65 | for i in range (size,2*size+1): 66 | if R[i] == 0: 67 | laser_depth_observations[i,0] = 100000 68 | laser_depth_observations[i,1] = 100000 69 | continue 70 | laser_depth_observations[i,0] = R[i] * np.cos((i-size) * angle_jumps) 71 | laser_depth_observations[i,1] = R[i] * np.sin((i-size) * angle_jumps) 72 | 73 | if i < angle_max and i >= angle_min: 74 | xyz = Point32() 75 | xyz.x = laser_depth_observations[i,0] 76 | xyz.y = laser_depth_observations[i,1] 77 | xyz.z = 0 78 | 79 | P.points.append(xyz) 80 | Object_P_pub.publish(P) 81 | return laser_depth_observations 82 | 83 | def _Rearrange_SSD_probability(Old_SSD_Probabilities): 84 | '''Return updated SSD probabilities''' 85 | Old_SSD_Probabilities = np.array(Old_SSD_Probabilities) 86 | global Object_cls_list 87 | New_SSD_Probabilities = np.zeros(20) 88 | Normalization_Factor = np.sum(Old_SSD_Probabilities[Object_cls_list-1]) 89 | for ii in (Object_cls_list-1): 90 | New_SSD_Probabilities[ii] = Old_SSD_Probabilities[ii]/Normalization_Factor 91 | 92 | return New_SSD_Probabilities 93 | 94 | def Object_height_update(height_factor,a,b,phi): 95 | phi = -phi 96 | '''Return the object height''' 97 | if phi < np.pi/2: 98 | object_x_width = a * np.cos(phi) + b * np.sin(phi) 99 | else: 100 | object_x_width = a * np.cos(np.pi - phi) + np.sin(phi-np.pi/2) 101 | return object_x_width * height_factor 102 | 103 | def quaternion_to_euler(x, y, z, w): 104 | '''Input quanterion angle. 105 | Return euler angles [Yaw,Pitch,Roll]''' 106 | t0 = +2.0 * (w * x + y * z) 107 | t1 = +1.0 - 2.0 * (x * x + y * y) 108 | roll = np.arctan2(t0, t1) 109 | t2 = +2.0 * (w * y - z * x) 110 | t2 = +1.0 if t2 > +1.0 else t2 111 | t2 = -1.0 if t2 < -1.0 else t2 112 | pitch = np.arcsin(t2) 113 | t3 = +2.0 * (w * z + x * y) 114 | t4 = +1.0 - 2.0 * (y * y + z * z) 115 | yaw = np.arctan2(t3, t4) 116 | return [yaw, pitch, roll] 117 | 118 | def GMM_Likelihood(mu,sigma,Z): 119 | '''Return sum of all the GMM density functions. 120 | Use when mu size is more than 1''' 121 | n = mu.shape[1] 122 | L = np.zeros(Z.shape[0]) 123 | for i in range(0,n): 124 | L += np.array(mn.pdf(Z,mean = mu[:,i],cov = sigma)) 125 | return np.sum(L/n) 126 | 127 | def GMM_Prior(mu,sigma,Z): 128 | '''Return sum of all the GMM density functions. 129 | Use when mu size is 1''' 130 | L = np.array(mn.pdf(Z,mean = mu,cov = sigma)) 131 | 132 | return np.sum(L) 133 | 134 | # R - robot, c - old center 135 | def New_Ce_array(x_c,y_c,x_R,y_R,yaw,correction): 136 | '''Rotation and translation from the robot view to map view. 137 | Return [x,y] array''' 138 | x = x_R + x_c*np.cos(yaw) - y_c*np.sin(yaw) + correction*np.cos(yaw) 139 | y = y_R + y_c*np.cos(yaw) + x_c*np.sin(yaw) + correction*np.sin(yaw) 140 | 141 | # Return the real pose of the point after changing it from the view of the laser: 142 | return np.array([x,y]) 143 | 144 | def _Init_Ellipse(theta): 145 | 146 | x0 = theta[0] 147 | y0 = theta[1] 148 | phi = theta[2] 149 | a = theta[3] 150 | b = theta[4] 151 | alpha = np.linspace(np.pi , 2 * np.pi, 5) 152 | x1 = a * np.cos(alpha) 153 | y1 = b * np.sin(alpha) 154 | x = x1 * np.cos(phi) - y1*np.sin(phi) + x0 155 | y = y1 * np.cos(phi) + x1*np.sin(phi) + y0 156 | Ellipse_vector = np.array([x,y]) 157 | return Ellipse_vector 158 | 159 | def _Distances_from_center_of_ellipse(theta,Z): 160 | '''Uses rotated and translated ellipse function on the depth observations. 161 | Return the value on all those observations. 162 | (If point is on the ellipse, return value of 1 for this point) ''' 163 | # Initializing and identifying variables: 164 | Distance_Vector = [] 165 | x_c = theta[0] 166 | y_c = theta[1] 167 | angle = theta[2] 168 | a = theta[3] 169 | b = theta[4] 170 | 171 | # Making a list of distances from the center of the ellipse: 172 | for z in Z: 173 | 174 | R_x = ((z[0]-x_c)*np.cos(angle) + (z[1]-y_c)*np.sin(angle))/(a) 175 | R_y = ((z[0]-x_c)*np.sin(angle) - (z[1]-y_c)*np.cos(angle))/(b) 176 | Distance_Vector.append(R_x**2 + R_y**2) 177 | 178 | return np.array(Distance_Vector) 179 | 180 | def _Initializing_half_Rectangle(theta): 181 | '''Return vector of points that will act as components of GMM for the rectangle. 182 | Return array of [x,y]''' 183 | x0 = theta[0] 184 | y0 = theta[1] 185 | phi = theta[2] 186 | a = theta[3] 187 | b = theta[4] 188 | 189 | if phi > 0 and phi < np.pi or phi < 0 and phi > -np.pi: 190 | 191 | b_a = int(10*b/(a+b)) 192 | x1 = ((np.cos(phi))/(np.absolute(np.cos(phi))))*(-a/2) * np.ones(b_a) 193 | y1 = np.linspace(-b/2 , b/2 , b_a) 194 | x2 = np.linspace(-a/2 , a/2 , 10 -b_a) 195 | y2 = -b/2 * np.ones(10-b_a) 196 | x3 = np.concatenate((x1,x2) , axis = None) 197 | y3 = np.concatenate((y1,y2) , axis = None) 198 | x = x3 * np.cos(phi) - y3 * np.sin(phi) + x0 199 | y = y3 * np.cos(phi) + x3 * np.sin(phi) + y0 200 | Rectangle_vector = np.array([x,y]).T 201 | 202 | elif phi == 0 or phi == np.pi/2 or phi == -np.pi/2: 203 | n = y0 - a/2 204 | x1 = x0 - a/2 205 | x2 = x0 + a/2 206 | x_v = np.linspace(x1,x2,7) 207 | y_v = np.ones(7) * n 208 | Rectangle_vector = np.array([x_v,y_v]).T 209 | 210 | else: 211 | x_v = np.linspace(0,100,7) 212 | y_v = np.ones(7) * 9999 213 | Rectangle_vector = np.array([x_v,y_v]).T 214 | 215 | 216 | return Rectangle_vector 217 | 218 | class Likelihood(): 219 | 220 | def __init__(self,class_number=[],Z=[],SSD_probability=[]): 221 | 222 | self.class_number = class_number 223 | self.Z = Z 224 | self.Probability = _Rearrange_SSD_probability(SSD_probability) 225 | 226 | # Likelihood functions: 227 | def probability_for_circle(self,theta): 228 | '''Return posterior for circle shape for a given theta''' 229 | 230 | R = theta[2] 231 | 232 | sigma = 0.0005 233 | sigma_prior = rospy.get_param('/object_list/o'+str(self.class_number) + '/cov') 234 | r_mean = rospy.get_param('/object_list/o'+str(self.class_number) + '/r') 235 | 236 | r_c = ( (theta[0]-self.Z[:,0])**2 + (theta[1]-self.Z[:,1])**2 )**0.5 237 | 238 | L = GMM_Prior(R,sigma,r_c) 239 | Prior = GMM_Prior(R,sigma_prior,r_mean) 240 | 241 | return -Prior * L * self.Probability[self.class_number-1] 242 | 243 | 244 | 245 | def probability_for_Rectangle(self,theta): 246 | '''Return posterior for rectangle shape for a given theta''' 247 | a = theta[3] 248 | b = theta[4] 249 | 250 | Rectangle_vector = _Initializing_half_Rectangle(theta).T 251 | 252 | sigma = np.array([[0.002,0],[0,0.002]]) 253 | sigma_prior = np.array(rospy.get_param('/object_list/o'+str(self.class_number) + '/cov')) 254 | L = GMM_Likelihood(Rectangle_vector,sigma,self.Z) 255 | 256 | 257 | a_mean = rospy.get_param('/object_list/o'+str(self.class_number) + '/a') 258 | b_mean = rospy.get_param('/object_list/o'+str(self.class_number) + '/b') 259 | Prior = GMM_Prior(np.array([a,b]),sigma_prior,np.array([a_mean,b_mean])) 260 | 261 | return -Prior* L * self.Probability[self.class_number-1] 262 | 263 | 264 | 265 | def probability_for_Ellipse(self,theta): 266 | '''Return posterior for ellipse shape for a given theta''' 267 | a = theta[3] 268 | b = theta[4] 269 | #Ellipse_vector = _Init_Ellipse(theta) 270 | Ellipse_vector = _Distances_from_center_of_ellipse(theta,self.Z) 271 | sigma = 0.09 272 | sigma_prior = np.array(rospy.get_param('/object_list/o'+str(self.class_number) + '/cov')) 273 | L = GMM_Prior(1,sigma,Ellipse_vector) 274 | a_mean = rospy.get_param('/object_list/o'+str(self.class_number) + '/a') 275 | b_mean = rospy.get_param('/object_list/o'+str(self.class_number) + '/b') 276 | 277 | Prior = GMM_Prior(np.array([a,b]),sigma_prior,np.array([a_mean,b_mean])) 278 | 279 | return -Prior * L * self.Probability[self.class_number-1] 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | -------------------------------------------------------------------------------- /komodo_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(komodo_demo) 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 | geometry_msgs 12 | message_generation 13 | message_runtime 14 | roscpp 15 | rospy 16 | std_msgs 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | # find_package(Boost REQUIRED COMPONENTS system) 21 | 22 | 23 | ## Uncomment this if the package has a setup.py. This macro ensures 24 | ## modules and global scripts declared therein get installed 25 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 26 | # catkin_python_setup() 27 | 28 | ################################################ 29 | ## Declare ROS messages, services and actions ## 30 | ################################################ 31 | 32 | ## To declare and build messages, services or actions from within this 33 | ## package, follow these steps: 34 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 35 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 36 | ## * In the file package.xml: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 39 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 40 | ## but can be declared for certainty nonetheless: 41 | ## * add a exec_depend tag for "message_runtime" 42 | ## * In this file (CMakeLists.txt): 43 | ## * add "message_generation" and every package in MSG_DEP_SET to 44 | ## find_package(catkin REQUIRED COMPONENTS ...) 45 | ## * add "message_runtime" and every package in MSG_DEP_SET to 46 | ## catkin_package(CATKIN_DEPENDS ...) 47 | ## * uncomment the add_*_files sections below as needed 48 | ## and list every .msg/.srv/.action file to be processed 49 | ## * uncomment the generate_messages entry below 50 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 51 | 52 | ## Generate messages in the 'msg' folder 53 | # add_message_files( 54 | # FILES 55 | # Message1.msg 56 | # Message2.msg 57 | # ) 58 | 59 | ## Generate services in the 'srv' folder 60 | # add_service_files( 61 | # FILES 62 | # Service1.srv 63 | # Service2.srv 64 | # ) 65 | 66 | ## Generate actions in the 'action' folder 67 | # add_action_files( 68 | # FILES 69 | # Action1.action 70 | # Action2.action 71 | # ) 72 | 73 | ## Generate added messages and services with any dependencies listed here 74 | # generate_messages( 75 | # DEPENDENCIES 76 | # geometry_msgs# std_msgs 77 | # ) 78 | 79 | ################################################ 80 | ## Declare ROS dynamic reconfigure parameters ## 81 | ################################################ 82 | 83 | ## To declare and build dynamic reconfigure parameters within this 84 | ## package, follow these steps: 85 | ## * In the file package.xml: 86 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 87 | ## * In this file (CMakeLists.txt): 88 | ## * add "dynamic_reconfigure" to 89 | ## find_package(catkin REQUIRED COMPONENTS ...) 90 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 91 | ## and list every .cfg file to be processed 92 | 93 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 94 | # generate_dynamic_reconfigure_options( 95 | # cfg/DynReconf1.cfg 96 | # cfg/DynReconf2.cfg 97 | # ) 98 | 99 | ################################### 100 | ## catkin specific configuration ## 101 | ################################### 102 | ## The catkin_package macro generates cmake config files for your package 103 | ## Declare things to be passed to dependent projects 104 | ## INCLUDE_DIRS: uncomment this if your package contains header files 105 | ## LIBRARIES: libraries you create in this project that dependent projects also need 106 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 107 | ## DEPENDS: system dependencies of this project that dependent projects also need 108 | catkin_package( 109 | # INCLUDE_DIRS include 110 | # LIBRARIES SLAM 111 | # CATKIN_DEPENDS geometry_msgs message_generation message_runtime roscpp rospy std_msgs 112 | # DEPENDS system_lib 113 | ) 114 | 115 | ########### 116 | ## Build ## 117 | ########### 118 | 119 | ## Specify additional locations of header files 120 | ## Your package locations should be listed before other locations 121 | include_directories( 122 | # include 123 | ${catkin_INCLUDE_DIRS} 124 | ) 125 | 126 | ## Declare a C++ library 127 | # add_library(${PROJECT_NAME} 128 | # src/${PROJECT_NAME}/SLAM.cpp 129 | # ) 130 | 131 | ## Add cmake target dependencies of the library 132 | ## as an example, code may need to be generated before libraries 133 | ## either from message generation or dynamic reconfigure 134 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 135 | 136 | ## Declare a C++ executable 137 | ## With catkin_make all packages are built within a single CMake context 138 | ## The recommended prefix ensures that target names across packages don't collide 139 | # add_executable(${PROJECT_NAME}_node src/SLAM_node.cpp) 140 | 141 | ## Rename C++ executable without prefix 142 | ## The above recommended prefix causes long target names, the following renames the 143 | ## target back to the shorter version for ease of user use 144 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 145 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 146 | 147 | ## Add cmake target dependencies of the executable 148 | ## same as for the library above 149 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 150 | 151 | ## Specify libraries to link a library or executable target against 152 | # target_link_libraries(${PROJECT_NAME}_node 153 | # ${catkin_LIBRARIES} 154 | # ) 155 | 156 | ############# 157 | ## Install ## 158 | ############# 159 | 160 | # all install targets should use catkin DESTINATION variables 161 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 162 | 163 | ## Mark executable scripts (Python etc.) for installation 164 | ## in contrast to setup.py, you can choose the destination 165 | # install(PROGRAMS 166 | # scripts/my_python_script 167 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | # ) 169 | 170 | ## Mark executables and/or libraries for installation 171 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_SLAM.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /komodo_demo/launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /komodo_demo/launch/hector_slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /komodo_demo/launch/joystick.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /komodo_demo/launch/object_map.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /komodo_demo/launch/ssd.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /komodo_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | komodo_demo 4 | 0.0.0 5 | The SLAM package 6 | 7 | 8 | 9 | 10 | lab 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 | geometry_msgs 53 | message_generation 54 | roscpp 55 | rospy 56 | std_msgs 57 | geometry_msgs 58 | roscpp 59 | rospy 60 | std_msgs 61 | geometry_msgs 62 | message_runtime 63 | roscpp 64 | rospy 65 | std_msgs 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /object_detector_ssd_tf_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(object_detector_ssd_tf_ros) 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 | std_msgs 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 | SSD_Output.msg 53 | SSD_Outputs.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 object_detector_ssd_tf_ros 108 | CATKIN_DEPENDS rospy std_msgs 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}/object_detector_ssd_tf_ros.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/object_detector_ssd_tf_ros_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 for installation 168 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 169 | # install(TARGETS ${PROJECT_NAME}_node 170 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | # ) 172 | 173 | ## Mark libraries for installation 174 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 175 | # install(TARGETS ${PROJECT_NAME} 176 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_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_object_detector_ssd_tf_ros.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 | -------------------------------------------------------------------------------- /object_detector_ssd_tf_ros/msg/SSD_Output.msg: -------------------------------------------------------------------------------- 1 | int16 x_min 2 | int16 x_max 3 | int16 y_min 4 | int16 y_max 5 | int16 cls 6 | float64 height_factor 7 | float64[] probability_distribution 8 | -------------------------------------------------------------------------------- /object_detector_ssd_tf_ros/msg/SSD_Outputs.msg: -------------------------------------------------------------------------------- 1 | SSD_Output[] outputs 2 | -------------------------------------------------------------------------------- /object_detector_ssd_tf_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | object_detector_ssd_tf_ros 4 | 0.0.1 5 | The object_detector_ssd_tf_ros package 6 | 7 | Amit Elbaz 8 | 9 | Or Tslil 10 | BSD 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | message_generation 35 | 36 | 37 | 38 | 39 | 40 | message_runtime 41 | 42 | 43 | 44 | 45 | catkin 46 | rospy 47 | std_msgs 48 | rospy 49 | std_msgs 50 | rospy 51 | std_msgs 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /object_detector_ssd_tf_ros/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | 3 | setup(name='object_detector_ssd_tf_ros', version='0.1', packages=find_packages()) 4 | -------------------------------------------------------------------------------- /object_detector_ssd_tf_ros/ssd/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/object_detector_ssd_tf_ros/ssd/__init__.py -------------------------------------------------------------------------------- /object_detector_ssd_tf_ros/ssd/model/labels.txt: -------------------------------------------------------------------------------- 1 | labels 2 | background 3 | aeroplane 4 | bicycle 5 | bird 6 | boat 7 | bottle 8 | bus 9 | car 10 | cat 11 | chair 12 | cow 13 | diningtable 14 | dog 15 | horse 16 | motorbike 17 | person 18 | pottedplant 19 | sheep 20 | sofa 21 | train 22 | tvmonitor 23 | -------------------------------------------------------------------------------- /object_detector_ssd_tf_ros/ssd/model/ssd_300_vgg.ckpt.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/object_detector_ssd_tf_ros/ssd/model/ssd_300_vgg.ckpt.zip -------------------------------------------------------------------------------- /object_detector_ssd_tf_ros/ssd/ssd/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/object_detector_ssd_tf_ros/ssd/ssd/__init__.py -------------------------------------------------------------------------------- /object_detector_ssd_tf_ros/ssd/ssd/custom_layers.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Paul Balanca. All Rights Reserved. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # ============================================================================== 15 | """Implement some custom layers, not provided by TensorFlow. 16 | 17 | Trying to follow as much as possible the style/standards used in 18 | tf.contrib.layers 19 | """ 20 | import tensorflow as tf 21 | 22 | from tensorflow.contrib.framework.python.ops import add_arg_scope 23 | from tensorflow.contrib.layers.python.layers import initializers 24 | from tensorflow.contrib.framework.python.ops import variables 25 | from tensorflow.contrib.layers.python.layers import utils 26 | from tensorflow.python.ops import nn 27 | from tensorflow.python.ops import init_ops 28 | from tensorflow.python.ops import variable_scope 29 | 30 | 31 | def abs_smooth(x): 32 | """Smoothed absolute function. Useful to compute an L1 smooth error. 33 | 34 | Define as: 35 | x^2 / 2 if abs(x) < 1 36 | abs(x) - 0.5 if abs(x) > 1 37 | We use here a differentiable definition using min(x) and abs(x). Clearly 38 | not optimal, but good enough for our purpose! 39 | """ 40 | absx = tf.abs(x) 41 | minx = tf.minimum(absx, 1) 42 | r = 0.5 * ((absx - 1) * minx + absx) 43 | return r 44 | 45 | 46 | @add_arg_scope 47 | def l2_normalization( 48 | inputs, 49 | scaling=False, 50 | scale_initializer=init_ops.ones_initializer(), 51 | reuse=None, 52 | variables_collections=None, 53 | outputs_collections=None, 54 | data_format='NHWC', 55 | trainable=True, 56 | scope=None): 57 | """Implement L2 normalization on every feature (i.e. spatial normalization). 58 | 59 | Should be extended in some near future to other dimensions, providing a more 60 | flexible normalization framework. 61 | 62 | Args: 63 | inputs: a 4-D tensor with dimensions [batch_size, height, width, channels]. 64 | scaling: whether or not to add a post scaling operation along the dimensions 65 | which have been normalized. 66 | scale_initializer: An initializer for the weights. 67 | reuse: whether or not the layer and its variables should be reused. To be 68 | able to reuse the layer scope must be given. 69 | variables_collections: optional list of collections for all the variables or 70 | a dictionary containing a different list of collection per variable. 71 | outputs_collections: collection to add the outputs. 72 | data_format: NHWC or NCHW data format. 73 | trainable: If `True` also add variables to the graph collection 74 | `GraphKeys.TRAINABLE_VARIABLES` (see tf.Variable). 75 | scope: Optional scope for `variable_scope`. 76 | Returns: 77 | A `Tensor` representing the output of the operation. 78 | """ 79 | 80 | with variable_scope.variable_scope( 81 | scope, 'L2Normalization', [inputs], reuse=reuse) as sc: 82 | inputs_shape = inputs.get_shape() 83 | inputs_rank = inputs_shape.ndims 84 | dtype = inputs.dtype.base_dtype 85 | if data_format == 'NHWC': 86 | # norm_dim = tf.range(1, inputs_rank-1) 87 | norm_dim = tf.range(inputs_rank-1, inputs_rank) 88 | params_shape = inputs_shape[-1:] 89 | elif data_format == 'NCHW': 90 | # norm_dim = tf.range(2, inputs_rank) 91 | norm_dim = tf.range(1, 2) 92 | params_shape = (inputs_shape[1]) 93 | 94 | # Normalize along spatial dimensions. 95 | outputs = nn.l2_normalize(inputs, norm_dim, epsilon=1e-12) 96 | # Additional scaling. 97 | if scaling: 98 | scale_collections = utils.get_variable_collections( 99 | variables_collections, 'scale') 100 | scale = variables.model_variable('gamma', 101 | shape=params_shape, 102 | dtype=dtype, 103 | initializer=scale_initializer, 104 | collections=scale_collections, 105 | trainable=trainable) 106 | if data_format == 'NHWC': 107 | outputs = tf.multiply(outputs, scale) 108 | elif data_format == 'NCHW': 109 | scale = tf.expand_dims(scale, axis=-1) 110 | scale = tf.expand_dims(scale, axis=-1) 111 | outputs = tf.multiply(outputs, scale) 112 | # outputs = tf.transpose(outputs, perm=(0, 2, 3, 1)) 113 | 114 | return utils.collect_named_outputs(outputs_collections, 115 | sc.original_name_scope, outputs) 116 | 117 | 118 | @add_arg_scope 119 | def pad2d(inputs, 120 | pad=(0, 0), 121 | mode='CONSTANT', 122 | data_format='NHWC', 123 | trainable=True, 124 | scope=None): 125 | """2D Padding layer, adding a symmetric padding to H and W dimensions. 126 | 127 | Aims to mimic padding in Caffe and MXNet, helping the port of models to 128 | TensorFlow. Tries to follow the naming convention of `tf.contrib.layers`. 129 | 130 | Args: 131 | inputs: 4D input Tensor; 132 | pad: 2-Tuple with padding values for H and W dimensions; 133 | mode: Padding mode. C.f. `tf.pad` 134 | data_format: NHWC or NCHW data format. 135 | """ 136 | with tf.name_scope(scope, 'pad2d', [inputs]): 137 | # Padding shape. 138 | if data_format == 'NHWC': 139 | paddings = [[0, 0], [pad[0], pad[0]], [pad[1], pad[1]], [0, 0]] 140 | elif data_format == 'NCHW': 141 | paddings = [[0, 0], [0, 0], [pad[0], pad[0]], [pad[1], pad[1]]] 142 | net = tf.pad(inputs, paddings, mode=mode) 143 | return net 144 | 145 | 146 | @add_arg_scope 147 | def channel_to_last(inputs, 148 | data_format='NHWC', 149 | scope=None): 150 | """Move the channel axis to the last dimension. Allows to 151 | provide a single output format whatever the input data format. 152 | 153 | Args: 154 | inputs: Input Tensor; 155 | data_format: NHWC or NCHW. 156 | Return: 157 | Input in NHWC format. 158 | """ 159 | with tf.name_scope(scope, 'channel_to_last', [inputs]): 160 | if data_format == 'NHWC': 161 | net = inputs 162 | elif data_format == 'NCHW': 163 | net = tf.transpose(inputs, perm=(0, 2, 3, 1)) 164 | return net 165 | -------------------------------------------------------------------------------- /object_detector_ssd_tf_ros/ssd/ssd/np_methods.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Paul Balanca. All Rights Reserved. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # ============================================================================== 15 | """Additional Numpy methods. Big mess of many things! 16 | """ 17 | import numpy as np 18 | 19 | 20 | # =========================================================================== # 21 | # Numpy implementations of SSD boxes functions. 22 | # =========================================================================== # 23 | def ssd_bboxes_decode(feat_localizations, 24 | anchor_bboxes, 25 | prior_scaling=[0.1, 0.1, 0.2, 0.2]): 26 | """Compute the relative bounding boxes from the layer features and 27 | reference anchor bounding boxes. 28 | 29 | Return: 30 | numpy array Nx4: ymin, xmin, ymax, xmax 31 | """ 32 | # Reshape for easier broadcasting. 33 | l_shape = feat_localizations.shape 34 | feat_localizations = np.reshape(feat_localizations, 35 | (-1, l_shape[-2], l_shape[-1])) 36 | yref, xref, href, wref = anchor_bboxes 37 | xref = np.reshape(xref, [-1, 1]) 38 | yref = np.reshape(yref, [-1, 1]) 39 | 40 | # Compute center, height and width 41 | cx = feat_localizations[:, :, 0] * wref * prior_scaling[0] + xref 42 | cy = feat_localizations[:, :, 1] * href * prior_scaling[1] + yref 43 | w = wref * np.exp(feat_localizations[:, :, 2] * prior_scaling[2]) 44 | h = href * np.exp(feat_localizations[:, :, 3] * prior_scaling[3]) 45 | # bboxes: ymin, xmin, xmax, ymax. 46 | bboxes = np.zeros_like(feat_localizations) 47 | bboxes[:, :, 0] = cy - h / 2. 48 | bboxes[:, :, 1] = cx - w / 2. 49 | bboxes[:, :, 2] = cy + h / 2. 50 | bboxes[:, :, 3] = cx + w / 2. 51 | # Back to original shape. 52 | bboxes = np.reshape(bboxes, l_shape) 53 | return bboxes 54 | 55 | 56 | def ssd_bboxes_select_layer(predictions_layer, 57 | localizations_layer, 58 | anchors_layer, 59 | select_threshold=0.5, 60 | img_shape=(300, 300), 61 | num_classes=21, 62 | decode=True): 63 | """Extract classes, scores and bounding boxes from features in one layer. 64 | 65 | Return: 66 | classes, scores, bboxes: Numpy arrays... 67 | """ 68 | # First decode localizations features if necessary. 69 | if decode: 70 | localizations_layer = ssd_bboxes_decode(localizations_layer, anchors_layer) 71 | 72 | # Reshape features to: Batches x N x N_labels | 4. 73 | p_shape = predictions_layer.shape 74 | batch_size = p_shape[0] if len(p_shape) == 5 else 1 75 | predictions_layer = np.reshape(predictions_layer, 76 | (batch_size, -1, p_shape[-1])) 77 | l_shape = localizations_layer.shape 78 | localizations_layer = np.reshape(localizations_layer, 79 | (batch_size, -1, l_shape[-1])) 80 | 81 | # Boxes selection: use threshold or score > no-label criteria. 82 | if select_threshold is None or select_threshold == 0: 83 | # Class prediction and scores: assign 0. to 0-class 84 | classes = np.argmax(predictions_layer, axis=2) 85 | scores = np.amax(predictions_layer, axis=2) 86 | mask = (classes > 0) 87 | classes = classes[mask] 88 | scores = scores[mask] 89 | bboxes = localizations_layer[mask] 90 | else: 91 | sub_predictions = predictions_layer[:, :, 1:] 92 | #print(predictions_layer.shape) 93 | idxes = np.where(sub_predictions > select_threshold) 94 | classes = idxes[-1]+1 95 | scores = sub_predictions[idxes] 96 | bboxes = localizations_layer[idxes[:-1]] 97 | #probs = np.squeeze(sub_predictions) #/np.sum(sub_predictions[-1]) 98 | probs = sub_predictions[idxes[:-1]] 99 | 100 | return classes, scores, bboxes , probs 101 | 102 | 103 | def ssd_bboxes_select(predictions_net, 104 | localizations_net, 105 | anchors_net, 106 | select_threshold=0.5, 107 | img_shape=(300, 300), 108 | num_classes=21, 109 | decode=True): 110 | """Extract classes, scores and bounding boxes from network output layers. 111 | 112 | Return: 113 | classes, scores, bboxes: Numpy arrays... 114 | """ 115 | l_classes = [] 116 | l_scores = [] 117 | l_bboxes = [] 118 | l_probs = [] 119 | # l_layers = [] 120 | # l_idxes = [] 121 | for i in range(len(predictions_net)): 122 | classes, scores, bboxes, probs = ssd_bboxes_select_layer( 123 | predictions_net[i], localizations_net[i], anchors_net[i], 124 | select_threshold, img_shape, num_classes, decode) 125 | l_classes.append(classes) 126 | l_scores.append(scores) 127 | l_bboxes.append(bboxes) 128 | l_probs.append(probs) 129 | # Debug information. 130 | # l_layers.append(i) 131 | # l_idxes.append((i, idxes)) 132 | 133 | classes = np.concatenate(l_classes, 0) 134 | scores = np.concatenate(l_scores, 0) 135 | bboxes = np.concatenate(l_bboxes, 0) 136 | l_probs = np.concatenate(l_probs, 0) 137 | return classes, scores, bboxes , l_probs 138 | 139 | 140 | # =========================================================================== # 141 | # Common functions for bboxes handling and selection. 142 | # =========================================================================== # 143 | def bboxes_sort(classes, scores, bboxes,probs, top_k=400): 144 | """Sort bounding boxes by decreasing order and keep only the top_k 145 | """ 146 | # if priority_inside: 147 | # inside = (bboxes[:, 0] > margin) & (bboxes[:, 1] > margin) & \ 148 | # (bboxes[:, 2] < 1-margin) & (bboxes[:, 3] < 1-margin) 149 | # idxes = np.argsort(-scores) 150 | # inside = inside[idxes] 151 | # idxes = np.concatenate([idxes[inside], idxes[~inside]]) 152 | idxes = np.argsort(-scores) 153 | classes = classes[idxes][:top_k] 154 | scores = scores[idxes][:top_k] 155 | bboxes = bboxes[idxes][:top_k] 156 | sprobs = probs[idxes][:top_k] 157 | return classes, scores, bboxes, sprobs 158 | 159 | 160 | def bboxes_clip(bbox_ref, bboxes): 161 | """Clip bounding boxes with respect to reference bbox. 162 | """ 163 | bboxes = np.copy(bboxes) 164 | bboxes = np.transpose(bboxes) 165 | bbox_ref = np.transpose(bbox_ref) 166 | bboxes[0] = np.maximum(bboxes[0], bbox_ref[0]) 167 | bboxes[1] = np.maximum(bboxes[1], bbox_ref[1]) 168 | bboxes[2] = np.minimum(bboxes[2], bbox_ref[2]) 169 | bboxes[3] = np.minimum(bboxes[3], bbox_ref[3]) 170 | bboxes = np.transpose(bboxes) 171 | return bboxes 172 | 173 | 174 | def bboxes_resize(bbox_ref, bboxes): 175 | """Resize bounding boxes based on a reference bounding box, 176 | assuming that the latter is [0, 0, 1, 1] after transform. 177 | """ 178 | bboxes = np.copy(bboxes) 179 | # Translate. 180 | bboxes[:, 0] -= bbox_ref[0] 181 | bboxes[:, 1] -= bbox_ref[1] 182 | bboxes[:, 2] -= bbox_ref[0] 183 | bboxes[:, 3] -= bbox_ref[1] 184 | # Resize. 185 | resize = [bbox_ref[2] - bbox_ref[0], bbox_ref[3] - bbox_ref[1]] 186 | bboxes[:, 0] /= resize[0] 187 | bboxes[:, 1] /= resize[1] 188 | bboxes[:, 2] /= resize[0] 189 | bboxes[:, 3] /= resize[1] 190 | return bboxes 191 | 192 | 193 | def bboxes_jaccard(bboxes1, bboxes2): 194 | """Computing jaccard index between bboxes1 and bboxes2. 195 | Note: bboxes1 and bboxes2 can be multi-dimensional, but should broacastable. 196 | """ 197 | bboxes1 = np.transpose(bboxes1) 198 | bboxes2 = np.transpose(bboxes2) 199 | # Intersection bbox and volume. 200 | int_ymin = np.maximum(bboxes1[0], bboxes2[0]) 201 | int_xmin = np.maximum(bboxes1[1], bboxes2[1]) 202 | int_ymax = np.minimum(bboxes1[2], bboxes2[2]) 203 | int_xmax = np.minimum(bboxes1[3], bboxes2[3]) 204 | 205 | int_h = np.maximum(int_ymax - int_ymin, 0.) 206 | int_w = np.maximum(int_xmax - int_xmin, 0.) 207 | int_vol = int_h * int_w 208 | # Union volume. 209 | vol1 = (bboxes1[2] - bboxes1[0]) * (bboxes1[3] - bboxes1[1]) 210 | vol2 = (bboxes2[2] - bboxes2[0]) * (bboxes2[3] - bboxes2[1]) 211 | jaccard = int_vol / (vol1 + vol2 - int_vol) 212 | return jaccard 213 | 214 | 215 | def bboxes_intersection(bboxes_ref, bboxes2): 216 | """Computing jaccard index between bboxes1 and bboxes2. 217 | Note: bboxes1 and bboxes2 can be multi-dimensional, but should broacastable. 218 | """ 219 | bboxes_ref = np.transpose(bboxes_ref) 220 | bboxes2 = np.transpose(bboxes2) 221 | # Intersection bbox and volume. 222 | int_ymin = np.maximum(bboxes_ref[0], bboxes2[0]) 223 | int_xmin = np.maximum(bboxes_ref[1], bboxes2[1]) 224 | int_ymax = np.minimum(bboxes_ref[2], bboxes2[2]) 225 | int_xmax = np.minimum(bboxes_ref[3], bboxes2[3]) 226 | 227 | int_h = np.maximum(int_ymax - int_ymin, 0.) 228 | int_w = np.maximum(int_xmax - int_xmin, 0.) 229 | int_vol = int_h * int_w 230 | # Union volume. 231 | vol = (bboxes_ref[2] - bboxes_ref[0]) * (bboxes_ref[3] - bboxes_ref[1]) 232 | score = int_vol / vol 233 | return score 234 | 235 | 236 | def bboxes_nms(classes, scores, bboxes,probs, nms_threshold=0.45): 237 | """Apply non-maximum selection to bounding boxes. 238 | """ 239 | keep_bboxes = np.ones(scores.shape, dtype=np.bool) 240 | for i in range(scores.size-1): 241 | if keep_bboxes[i]: 242 | # Computer overlap with bboxes which are following. 243 | overlap = bboxes_jaccard(bboxes[i], bboxes[(i+1):]) 244 | # Overlap threshold for keeping + checking part of the same class 245 | keep_overlap = np.logical_or(overlap < nms_threshold, classes[(i+1):] != classes[i]) 246 | keep_bboxes[(i+1):] = np.logical_and(keep_bboxes[(i+1):], keep_overlap) 247 | 248 | idxes = np.where(keep_bboxes) 249 | return classes[idxes], scores[idxes], bboxes[idxes], probs[idxes] 250 | 251 | 252 | def bboxes_nms_fast(classes, scores, bboxes, threshold=0.45): 253 | """Apply non-maximum selection to bounding boxes. 254 | """ 255 | pass 256 | 257 | 258 | 259 | 260 | -------------------------------------------------------------------------------- /object_detector_ssd_tf_ros/ssd/ssd/tf_image.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/object_detector_ssd_tf_ros/ssd/ssd/tf_image.pyc -------------------------------------------------------------------------------- /object_detector_ssd_tf_ros/ssd/ssd_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import os 4 | import math 5 | import random 6 | import pandas as pd 7 | import numpy as np 8 | import tensorflow as tf 9 | import cv2 10 | from cv_bridge import CvBridge, CvBridgeError 11 | import rospy 12 | from sensor_msgs.msg import Image 13 | from object_detector_ssd_tf_ros.msg import SSD_Output,SSD_Outputs 14 | from std_msgs.msg import Float64 15 | import rospkg 16 | 17 | 18 | #slim = tf.contrib.slim 19 | 20 | import matplotlib.pyplot as plt 21 | import matplotlib.image as mpimg 22 | 23 | 24 | import ssd_wrapper 25 | # TensorFlow session: grow memory when needed. TF, DO NOT USE ALL MY GPU MEMORY!!! 26 | gpu_options = tf.GPUOptions(allow_growth=True) 27 | config = tf.ConfigProto(log_device_placement=False, gpu_options=gpu_options) 28 | 29 | ssd = ssd_wrapper.ssdWrapper(config = config) 30 | 31 | rospack = rospkg.RosPack() 32 | # get the file path for rospy_tutorials 33 | pa = rospack.get_path('object_detector_ssd_tf_ros') 34 | 35 | labels = pd.read_csv(pa+'/ssd/model/labels.txt') 36 | colors = dict() 37 | for cls_id in range(len(np.array(labels))): 38 | colors[cls_id] = (int(random.random()*255), int(random.random()*255), int(random.random()*255)) 39 | 40 | 41 | 42 | cv_img = 0 43 | 44 | def img_callback (ros_img): 45 | global bridge, cv_img 46 | try: 47 | cv_img = bridge.imgmsg_to_cv2(ros_img,"bgr8") 48 | except CvBridgeError as e: 49 | print (e) 50 | 51 | bridge = CvBridge() 52 | 53 | rospy.init_node('ssd_node', anonymous = True) 54 | img_sub = rospy.Subscriber('camera/image_raw/', Image, img_callback) 55 | ros_img = rospy.wait_for_message('camera/image_raw/', Image) 56 | Im_outs_Pub = rospy.Publisher('im_info',SSD_Outputs) 57 | img_pub = rospy.Publisher('ssd_image_output',Image) 58 | rate = rospy.Rate(5) 59 | 60 | 61 | 62 | while not rospy.is_shutdown(): 63 | #ret_val, img = cam.read() 64 | #global cv_img 65 | L_output = SSD_Outputs() 66 | flag = 0 67 | img = cv_img 68 | img = cv2.resize(img, (300, 300),interpolation = cv2.INTER_AREA) 69 | rclasses, rscores, rbboxes, probs = ssd.process_image(img) 70 | height = img.shape[0] 71 | width = img.shape[1] 72 | 73 | for i in range(len(rclasses)): 74 | output = SSD_Output() 75 | cls_id = int(rclasses[i]) 76 | if cls_id >= 0: 77 | ymin = int(rbboxes[i, 0] * height) 78 | xmin = int(rbboxes[i, 1] * width) 79 | ymax = int(rbboxes[i, 2] * height) 80 | xmax = int(rbboxes[i, 3] * width) 81 | flag = 1 82 | 83 | output.x_min = xmin 84 | output.x_max = xmax 85 | output.y_min = ymin 86 | output.y_max = ymax 87 | output.height_factor = (ymax-ymin)/(xmax-xmin) 88 | output.cls = cls_id 89 | 90 | 91 | for p in probs[i]: 92 | output.probability_distribution.append(p) 93 | 94 | L_output.outputs.append(output) 95 | 96 | 97 | img = cv2.rectangle(img,(xmin,ymin),(xmax,ymax),colors[cls_id],2) 98 | font = cv2.FONT_HERSHEY_SIMPLEX 99 | bottomLeftCornerOfText = (xmin,ymin + 20) 100 | fontScale = 1 101 | fontColor =colors[cls_id] 102 | lineType = 2 103 | 104 | img = cv2.putText(img,str(labels.iloc[cls_id][0]), 105 | bottomLeftCornerOfText, 106 | font, 107 | fontScale, 108 | fontColor, 109 | lineType) 110 | 111 | if flag == 0: 112 | output = SSD_Output() 113 | output.x_min = -1 114 | output.x_max = -1 115 | output.cls = -1 116 | output.y_min = -1 117 | output.y_max = -1 118 | L_output.outputs.append(output) 119 | 120 | Im_outs_Pub.publish(L_output) 121 | #cv2.imshow('SSD output', img) 122 | 123 | msg_frame = bridge.cv2_to_imgmsg(img) 124 | msg_frame.encoding = 'bgr8' 125 | msg_frame.header.frame_id = 'camera_link' 126 | img_pub.publish(msg_frame) 127 | rate.sleep() 128 | ''' 129 | if cv2.waitKey(1) == 27: 130 | cam.release() 131 | cv2.destroyAllWindows() 132 | break # esc to quit 133 | ''' 134 | 135 | 136 | -------------------------------------------------------------------------------- /object_detector_ssd_tf_ros/ssd/ssd_wrapper.py: -------------------------------------------------------------------------------- 1 | 2 | import os 3 | import math 4 | import random 5 | import pandas as pd 6 | import numpy as np 7 | import tensorflow as tf 8 | import cv2 9 | import rospkg 10 | 11 | slim = tf.contrib.slim 12 | 13 | rospack = rospkg.RosPack() 14 | # get the file path for rospy_tutorials 15 | pa = rospack.get_path('object_detector_ssd_tf_ros') 16 | 17 | ckpt_filename = pa+'/ssd/model/ssd_300_vgg.ckpt' 18 | 19 | from ssd import ssd_vgg_300, ssd_common, np_methods, ssd_vgg_preprocessing 20 | 21 | 22 | class ssdWrapper(): 23 | def __init__(self , config, net_shape = (300, 300), data_format = 'NHWC', ckpt_filename = ckpt_filename): 24 | self.isess = tf.InteractiveSession(config=config) 25 | # Input placeholder. 26 | self.net_shape = net_shape 27 | self.img_input = tf.placeholder(tf.uint8, shape=(None, None, 3)) 28 | # Evaluation pre-processing: resize to SSD net shape. 29 | image_pre, _, _, self.bbox_img = ssd_vgg_preprocessing.preprocess_for_eval( 30 | self.img_input, None, None, net_shape, data_format, resize=ssd_vgg_preprocessing.Resize.WARP_RESIZE) 31 | self.image_4d = tf.expand_dims(image_pre, 0) 32 | 33 | # Define the SSD model. 34 | 35 | reuse = True if 'ssd_net' in locals() else None 36 | ssd_net = ssd_vgg_300.SSDNet() 37 | with slim.arg_scope(ssd_net.arg_scope(data_format=data_format)): 38 | self.predictions, self.localisations, _, _ = ssd_net.net(self.image_4d, is_training=False, reuse=reuse) 39 | 40 | # Restore SSD model. 41 | self.isess.run(tf.global_variables_initializer()) 42 | saver = tf.train.Saver() 43 | print(ckpt_filename) 44 | saver.restore(self.isess, ckpt_filename) 45 | 46 | # SSD default anchor boxes. 47 | self.ssd_anchors = ssd_net.anchors(net_shape) 48 | 49 | 50 | 51 | 52 | # Main image processing routine. 53 | def process_image(self, img, select_threshold=0.5, nms_threshold=.45): 54 | # Run SSD network. 55 | _, rpredictions, rlocalisations, rbbox_img = self.isess.run([self.image_4d, self.predictions, self.localisations, self.bbox_img], feed_dict={self.img_input: img}) 56 | 57 | # Get classes and bboxes from the net outputs. 58 | rclasses, rscores, rbboxes , lprobs = np_methods.ssd_bboxes_select( 59 | rpredictions, rlocalisations, self.ssd_anchors, 60 | select_threshold=select_threshold, img_shape=self.net_shape, num_classes=21, decode=True) 61 | #print(lprobs.shape) 62 | rbboxes = np_methods.bboxes_clip(rbbox_img, rbboxes) 63 | rclasses, rscores, rbboxes, rprobs = np_methods.bboxes_sort(rclasses, rscores, rbboxes,lprobs, top_k=400) 64 | rclasses, rscores, rbboxes, rprobs = np_methods.bboxes_nms(rclasses, rscores, rbboxes,rprobs, nms_threshold=nms_threshold) 65 | #print(rprobs) 66 | # Resize bboxes to original image shape. Note: useless for Resize.WARP! 67 | rbboxes = np_methods.bboxes_resize(rbbox_img, rbboxes) 68 | return rclasses, rscores, rbboxes, rprobs 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /object_detector_ssd_tf_ros/ssd/tf_extended/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Paul Balanca. All Rights Reserved. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # ============================================================================== 15 | """TF Extended: additional metrics. 16 | """ 17 | 18 | # pylint: disable=unused-import,line-too-long,g-importing-member,wildcard-import 19 | from tf_extended.metrics import * 20 | from tf_extended.tensors import * 21 | from tf_extended.bboxes import * 22 | from tf_extended.image import * 23 | from tf_extended.math import * 24 | 25 | -------------------------------------------------------------------------------- /object_detector_ssd_tf_ros/ssd/tf_extended/image.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/object_detector_ssd_tf_ros/ssd/tf_extended/image.py -------------------------------------------------------------------------------- /object_detector_ssd_tf_ros/ssd/tf_extended/math.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Paul Balanca. All Rights Reserved. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # ============================================================================== 15 | """TF Extended: additional math functions. 16 | """ 17 | import tensorflow as tf 18 | 19 | from tensorflow.python.ops import array_ops 20 | from tensorflow.python.ops import math_ops 21 | from tensorflow.python.framework import dtypes 22 | from tensorflow.python.framework import ops 23 | 24 | 25 | def safe_divide(numerator, denominator, name): 26 | """Divides two values, returning 0 if the denominator is <= 0. 27 | Args: 28 | numerator: A real `Tensor`. 29 | denominator: A real `Tensor`, with dtype matching `numerator`. 30 | name: Name for the returned op. 31 | Returns: 32 | 0 if `denominator` <= 0, else `numerator` / `denominator` 33 | """ 34 | return tf.where( 35 | math_ops.greater(denominator, 0), 36 | math_ops.divide(numerator, denominator), 37 | tf.zeros_like(numerator), 38 | name=name) 39 | 40 | 41 | def cummax(x, reverse=False, name=None): 42 | """Compute the cumulative maximum of the tensor `x` along `axis`. This 43 | operation is similar to the more classic `cumsum`. Only support 1D Tensor 44 | for now. 45 | 46 | Args: 47 | x: A `Tensor`. Must be one of the following types: `float32`, `float64`, 48 | `int64`, `int32`, `uint8`, `uint16`, `int16`, `int8`, `complex64`, 49 | `complex128`, `qint8`, `quint8`, `qint32`, `half`. 50 | axis: A `Tensor` of type `int32` (default: 0). 51 | reverse: A `bool` (default: False). 52 | name: A name for the operation (optional). 53 | Returns: 54 | A `Tensor`. Has the same type as `x`. 55 | """ 56 | with ops.name_scope(name, "Cummax", [x]) as name: 57 | x = ops.convert_to_tensor(x, name="x") 58 | # Not very optimal: should directly integrate reverse into tf.scan. 59 | if reverse: 60 | x = tf.reverse(x, axis=[0]) 61 | # 'Accumlating' maximum: ensure it is always increasing. 62 | cmax = tf.scan(lambda a, y: tf.maximum(a, y), x, 63 | initializer=None, parallel_iterations=1, 64 | back_prop=False, swap_memory=False) 65 | if reverse: 66 | cmax = tf.reverse(cmax, axis=[0]) 67 | return cmax 68 | -------------------------------------------------------------------------------- /object_detector_ssd_tf_ros/ssd/tf_extended/tensors.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Paul Balanca. All Rights Reserved. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # ============================================================================== 15 | """TF Extended: additional tensors operations. 16 | """ 17 | import tensorflow as tf 18 | 19 | from tensorflow.contrib.framework.python.ops import variables as contrib_variables 20 | from tensorflow.contrib.metrics.python.ops import set_ops 21 | from tensorflow.python.framework import dtypes 22 | from tensorflow.python.framework import ops 23 | from tensorflow.python.framework import sparse_tensor 24 | from tensorflow.python.ops import array_ops 25 | from tensorflow.python.ops import check_ops 26 | from tensorflow.python.ops import control_flow_ops 27 | from tensorflow.python.ops import math_ops 28 | from tensorflow.python.ops import nn 29 | from tensorflow.python.ops import state_ops 30 | from tensorflow.python.ops import variable_scope 31 | from tensorflow.python.ops import variables 32 | 33 | 34 | def get_shape(x, rank=None): 35 | """Returns the dimensions of a Tensor as list of integers or scale tensors. 36 | 37 | Args: 38 | x: N-d Tensor; 39 | rank: Rank of the Tensor. If None, will try to guess it. 40 | Returns: 41 | A list of `[d1, d2, ..., dN]` corresponding to the dimensions of the 42 | input tensor. Dimensions that are statically known are python integers, 43 | otherwise they are integer scalar tensors. 44 | """ 45 | if x.get_shape().is_fully_defined(): 46 | return x.get_shape().as_list() 47 | else: 48 | static_shape = x.get_shape() 49 | if rank is None: 50 | static_shape = static_shape.as_list() 51 | rank = len(static_shape) 52 | else: 53 | static_shape = x.get_shape().with_rank(rank).as_list() 54 | dynamic_shape = tf.unstack(tf.shape(x), rank) 55 | return [s if s is not None else d 56 | for s, d in zip(static_shape, dynamic_shape)] 57 | 58 | 59 | def pad_axis(x, offset, size, axis=0, name=None): 60 | """Pad a tensor on an axis, with a given offset and output size. 61 | The tensor is padded with zero (i.e. CONSTANT mode). Note that the if the 62 | `size` is smaller than existing size + `offset`, the output tensor 63 | was the latter dimension. 64 | 65 | Args: 66 | x: Tensor to pad; 67 | offset: Offset to add on the dimension chosen; 68 | size: Final size of the dimension. 69 | Return: 70 | Padded tensor whose dimension on `axis` is `size`, or greater if 71 | the input vector was larger. 72 | """ 73 | with tf.name_scope(name, 'pad_axis'): 74 | shape = get_shape(x) 75 | rank = len(shape) 76 | # Padding description. 77 | new_size = tf.maximum(size-offset-shape[axis], 0) 78 | pad1 = tf.stack([0]*axis + [offset] + [0]*(rank-axis-1)) 79 | pad2 = tf.stack([0]*axis + [new_size] + [0]*(rank-axis-1)) 80 | paddings = tf.stack([pad1, pad2], axis=1) 81 | x = tf.pad(x, paddings, mode='CONSTANT') 82 | # Reshape, to get fully defined shape if possible. 83 | # TODO: fix with tf.slice 84 | shape[axis] = size 85 | x = tf.reshape(x, tf.stack(shape)) 86 | return x 87 | 88 | 89 | # def select_at_index(idx, val, t): 90 | # """Return a tensor. 91 | # """ 92 | # idx = tf.expand_dims(tf.expand_dims(idx, 0), 0) 93 | # val = tf.expand_dims(val, 0) 94 | # t = t + tf.scatter_nd(idx, val, tf.shape(t)) 95 | # return t 96 | -------------------------------------------------------------------------------- /object_mapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(object_mapping) 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 | std_msgs 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 | Object.msg 53 | Object_Map.msg 54 | Single_Class.msg 55 | M_i.msg 56 | M.msg 57 | ) 58 | 59 | ## Generate services in the 'srv' folder 60 | # add_service_files( 61 | # FILES 62 | # Service1.srv 63 | # Service2.srv 64 | # ) 65 | 66 | ## Generate actions in the 'action' folder 67 | # add_action_files( 68 | # FILES 69 | # Action1.action 70 | # Action2.action 71 | # ) 72 | 73 | ## Generate added messages and services with any dependencies listed here 74 | generate_messages( 75 | DEPENDENCIES 76 | std_msgs 77 | ) 78 | 79 | ################################################ 80 | ## Declare ROS dynamic reconfigure parameters ## 81 | ################################################ 82 | 83 | ## To declare and build dynamic reconfigure parameters within this 84 | ## package, follow these steps: 85 | ## * In the file package.xml: 86 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 87 | ## * In this file (CMakeLists.txt): 88 | ## * add "dynamic_reconfigure" to 89 | ## find_package(catkin REQUIRED COMPONENTS ...) 90 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 91 | ## and list every .cfg file to be processed 92 | 93 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 94 | # generate_dynamic_reconfigure_options( 95 | # cfg/DynReconf1.cfg 96 | # cfg/DynReconf2.cfg 97 | # ) 98 | 99 | ################################### 100 | ## catkin specific configuration ## 101 | ################################### 102 | ## The catkin_package macro generates cmake config files for your package 103 | ## Declare things to be passed to dependent projects 104 | ## INCLUDE_DIRS: uncomment this if your package contains header files 105 | ## LIBRARIES: libraries you create in this project that dependent projects also need 106 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 107 | ## DEPENDS: system dependencies of this project that dependent projects also need 108 | catkin_package( 109 | # INCLUDE_DIRS include 110 | # LIBRARIES object_mapping 111 | CATKIN_DEPENDS message_runtime rospy std_msgs 112 | # DEPENDS system_lib 113 | ) 114 | 115 | ########### 116 | ## Build ## 117 | ########### 118 | 119 | ## Specify additional locations of header files 120 | ## Your package locations should be listed before other locations 121 | include_directories( 122 | # include 123 | ${catkin_INCLUDE_DIRS} 124 | ) 125 | 126 | ## Declare a C++ library 127 | # add_library(${PROJECT_NAME} 128 | # src/${PROJECT_NAME}/object_mapping.cpp 129 | # ) 130 | 131 | ## Add cmake target dependencies of the library 132 | ## as an example, code may need to be generated before libraries 133 | ## either from message generation or dynamic reconfigure 134 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 135 | 136 | ## Declare a C++ executable 137 | ## With catkin_make all packages are built within a single CMake context 138 | ## The recommended prefix ensures that target names across packages don't collide 139 | # add_executable(${PROJECT_NAME}_node src/object_mapping_node.cpp) 140 | 141 | ## Rename C++ executable without prefix 142 | ## The above recommended prefix causes long target names, the following renames the 143 | ## target back to the shorter version for ease of user use 144 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 145 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 146 | 147 | ## Add cmake target dependencies of the executable 148 | ## same as for the library above 149 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 150 | 151 | ## Specify libraries to link a library or executable target against 152 | # target_link_libraries(${PROJECT_NAME}_node 153 | # ${catkin_LIBRARIES} 154 | # ) 155 | 156 | ############# 157 | ## Install ## 158 | ############# 159 | 160 | # all install targets should use catkin DESTINATION variables 161 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 162 | 163 | ## Mark executable scripts (Python etc.) for installation 164 | ## in contrast to setup.py, you can choose the destination 165 | # install(PROGRAMS 166 | # scripts/my_python_script 167 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | # ) 169 | 170 | ## Mark executables for installation 171 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 172 | # install(TARGETS ${PROJECT_NAME}_node 173 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 174 | # ) 175 | 176 | ## Mark libraries for installation 177 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 178 | # install(TARGETS ${PROJECT_NAME} 179 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 180 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 181 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 182 | # ) 183 | 184 | ## Mark cpp header files for installation 185 | # install(DIRECTORY include/${PROJECT_NAME}/ 186 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 187 | # FILES_MATCHING PATTERN "*.h" 188 | # PATTERN ".svn" EXCLUDE 189 | # ) 190 | 191 | ## Mark other files for installation (e.g. launch and bag files, etc.) 192 | # install(FILES 193 | # # myfile1 194 | # # myfile2 195 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 196 | # ) 197 | 198 | ############# 199 | ## Testing ## 200 | ############# 201 | 202 | ## Add gtest based cpp test target and link libraries 203 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_object_mapping.cpp) 204 | # if(TARGET ${PROJECT_NAME}-test) 205 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 206 | # endif() 207 | 208 | ## Add folders to be run by python nosetests 209 | # catkin_add_nosetests(test) 210 | -------------------------------------------------------------------------------- /object_mapping/config/theta_cov.yaml: -------------------------------------------------------------------------------- 1 | # cov 2 | epsilon: 0.0005 3 | o2: #bicycle 4 | - [0.1,0,0,0,0] 5 | - [0,0.04,0,0,0] 6 | - [0,0,0.01,0,0] 7 | - [0,0,0,0.2,0] 8 | - [0,0,0,0,0.01] 9 | o3: #bird 10 | - [0.03,0,0,0,0] 11 | - [0,0.02,0,0,0] 12 | - [0,0,0.02,0,0] 13 | - [0,0,0,0.02,0] 14 | - [0,0,0,0,0.01] 15 | o5: #bottle 16 | - [0.01,0,0] 17 | - [0,0.01,0] 18 | - [0,0,0.015] 19 | o8: #cat 20 | - [0.1,0,0,0,0] 21 | - [0,0.07,0,0,0] 22 | - [0,0,0.1,0,0] 23 | - [0,0,0,0.04,0] 24 | - [0,0,0,0,0.02] 25 | o9: #chair 26 | - [0.1,0,0,0,0] 27 | - [0,0.1,0,0,0] 28 | - [0,0,0.02,0,0] 29 | - [0,0,0,0.125,0] 30 | - [0,0,0,0,0.125] 31 | o12: #dog 32 | - [0.12,0,0,0,0] 33 | - [0,0.1,0,0,0] 34 | - [0,0,0.1,0,0] 35 | - [0,0,0,0.05,0] 36 | - [0,0,0,0,0.03] 37 | o14: #motorbike 38 | - [0.15,0,0,0,0] 39 | - [0,0.1,0,0,0] 40 | - [0,0,0.02,0,0] 41 | - [0,0,0,0.2,0] 42 | - [0,0,0,0,0.01] 43 | o15: #person 44 | - [0.09,0,0,0,0] 45 | - [0,0.09,0,0,0] 46 | - [0,0,0.02,0,0] 47 | - [0,0,0,0.09,0] 48 | - [0,0,0,0,0.03] 49 | o16: #pottedplant 50 | - [0.01,0,0] 51 | - [0,0.01,0] 52 | - [0,0,0.05] 53 | o17: #sheep 54 | - [0.1,0,0,0,0] 55 | - [0,0.1,0,0,0] 56 | - [0,0,0.02,0,0] 57 | - [0,0,0,0.2,0] 58 | - [0,0,0,0,0.05] 59 | o18: #sofa 60 | - [0.2,0,0,0,0] 61 | - [0,0.1,0,0,0] 62 | - [0,0,0.02,0,0] 63 | - [0,0,0,0.5,0] 64 | - [0,0,0,0,0.02] 65 | o20: #tvmonitor 66 | - [0.1,0,0,0,0] 67 | - [0,0.04,0,0,0] 68 | - [0,0,0.08,0,0] 69 | - [0,0,0,0.1,0] 70 | - [0,0,0,0,0.01] 71 | -------------------------------------------------------------------------------- /object_mapping/msg/M.msg: -------------------------------------------------------------------------------- 1 | M_i[] M 2 | -------------------------------------------------------------------------------- /object_mapping/msg/M_i.msg: -------------------------------------------------------------------------------- 1 | Single_Class[] m_i 2 | -------------------------------------------------------------------------------- /object_mapping/msg/Object.msg: -------------------------------------------------------------------------------- 1 | float64 x_center 2 | float64 y_center 3 | 4 | float64 r 5 | 6 | float64 a 7 | float64 b 8 | float64 angle 9 | 10 | float64 height_factor 11 | float64 object_height 12 | 13 | float64[] probabilities 14 | int16 cls_num 15 | 16 | 17 | -------------------------------------------------------------------------------- /object_mapping/msg/Object_Map.msg: -------------------------------------------------------------------------------- 1 | Object[] object_map 2 | -------------------------------------------------------------------------------- /object_mapping/msg/Single_Class.msg: -------------------------------------------------------------------------------- 1 | float64 x_center 2 | float64 y_center 3 | 4 | float64 r 5 | 6 | float64 a 7 | float64 b 8 | float64 angle 9 | 10 | float64 height_factor 11 | float64 object_height 12 | 13 | float64 probability 14 | int16 cls_num 15 | 16 | 17 | -------------------------------------------------------------------------------- /object_mapping/object_mapper/H_functions.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Libraries: 4 | import rospy 5 | import numpy as np 6 | import pandas as pd 7 | from scipy.stats import multivariate_normal as mn 8 | 9 | 10 | # Msgs: 11 | from object_mapping.msg import Single_Class , M_i , M 12 | 13 | # Yamls: 14 | A_Round = rospy.get_param('/Array/round') 15 | 16 | def q_i(mi,mo): 17 | '''Calculating q_i.''' 18 | global A_Round 19 | q_i = 0 20 | for ii in range(0,12): 21 | if mi.cls_num[ii] in A_Round: 22 | theta_i = np.array([mi.x_center[ii],mi.y_center[ii],mi.r[ii]]) 23 | theta_o = np.array([mo.x_center[ii],mo.y_center[ii],mo.r[ii]]) 24 | else: 25 | theta_i = np.array([mi.x_center[ii],mi.y_center[ii],mi.angle[ii],mi.a[ii],mi.b[ii]]) 26 | theta_o = np.array([mo.x_center[ii],mo.y_center[ii],mo.angle[ii],mo.a[ii],mi.b[ii]]) 27 | 28 | sigma = np.array(rospy.get_param('/Cov/o' + str(mi.cls_num[ii]))) 29 | f_mi_mo = mn.pdf(theta_o, mean = theta_i ,cov = sigma) 30 | 31 | q_i += mi.prob_distribution[ii] * mo.prob_distribution[ii] * f_mi_mo 32 | return q_i 33 | 34 | def _Making_M_i(mi): 35 | '''Initialize Mi msg from Mi class. 36 | Return msg from M_i() type.''' 37 | M_i_list = M_i() 38 | for i in range(0,12): 39 | SO = Single_Class() 40 | SO.x_center = mi.x_center[i] 41 | SO.y_center = mi.y_center[i] 42 | SO.r = mi.r[i] 43 | SO.a = mi.a[i] 44 | SO.b = mi.b[i] 45 | SO.angle = mi.angle[i] 46 | SO.cls_num = mi.cls_num[i] 47 | SO.probability = mi.prob_distribution[i] 48 | SO.object_height = mi.object_height[i] 49 | M_i_list.m_i.append(SO) 50 | 51 | return M_i_list 52 | 53 | def making_M(M_class_list): 54 | '''Making M msg from M class. 55 | Reruen msg from M() type.''' 56 | M_msg_list = M() 57 | for ii in range (0,len(M_class_list)): 58 | M_msg_list.M.append(_Making_M_i(M_class_list[ii])) 59 | 60 | return M_msg_list 61 | 62 | def Prob_updater(alpha,P_old,P_new): 63 | '''Updating probability of an object. 64 | Returns updated probability''' 65 | P_star = (P_old**alpha) * (P_new**(1-alpha)) 66 | P_sum = np.sum(P_star) 67 | P = P_star / P_sum 68 | return P 69 | 70 | # Single object class: 71 | class SO_class(): 72 | 73 | def __init__(self,x_center=[],y_center=[],r=[],a=[],b=[],angle=[],cls_num=[],prob_distribution=[],object_height = []): 74 | 75 | self.x_center = np.array(x_center) 76 | self.y_center = np.array(y_center) 77 | self.r = np.array(r) 78 | self.a = np.array(a) 79 | self.b = np.array(b) 80 | self.angle = np.array(angle) 81 | self.cls_num = np.array(cls_num) 82 | self.prob_distribution = np.array(prob_distribution) 83 | self.object_height = np.array(object_height) 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /object_mapping/object_mapper/H_functions.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/object_mapping/object_mapper/H_functions.pyc -------------------------------------------------------------------------------- /object_mapping/object_mapper/Object_Mapper.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Libraries: 4 | import rospy 5 | import numpy as np 6 | from obj_class import Updated_Probabilities_and_Cls,closest_node,Object_Map_cls,Search_Radius,Theta_updater 7 | import pandas as pd 8 | 9 | # Msgs: 10 | from joint_object_localizer.msg import OG_List 11 | from object_mapping.msg import Object,Object_Map 12 | 13 | global Theta_list 14 | 15 | def OG_callback(data): 16 | 17 | # The subs: 18 | global Theta_list 19 | Theta_list = data 20 | 21 | 22 | # Initial node: 23 | rospy.init_node('List_of_mapped_objects', anonymous=True) 24 | # Publishers: 25 | OM_publisher = rospy.Publisher('/object_mapped_values',Object_Map,queue_size=10) 26 | A_Rectangle = rospy.get_param('/Array/rectangle') 27 | A_Elliptical = rospy.get_param('/Array/elliptical') 28 | 29 | rr = rospy.Rate(10) 30 | object_class_list = [] 31 | object_mapped_values = Object_Map() 32 | objects_center = np.array([0,0]) 33 | 34 | while not rospy.is_shutdown(): 35 | 36 | # Subscribers: 37 | Theta_list_sub = rospy.Subscriber('/Theta_List',OG_List,OG_callback,queue_size=7) 38 | rospy.wait_for_message('/Theta_List',OG_List) 39 | 40 | data = Theta_list 41 | 42 | for ii in range(0,len(data.object_list)): 43 | 44 | 45 | # Getting the new data: 46 | obj_new = Object() 47 | obj_new.x_center = data.object_list[ii].x_center 48 | obj_new.y_center = data.object_list[ii].y_center 49 | obj_new.r = data.object_list[ii].r 50 | obj_new.a = data.object_list[ii].a 51 | obj_new.b = data.object_list[ii].b 52 | obj_new.angle = data.object_list[ii].angle 53 | obj_new.probabilities = data.object_list[ii].probabilities 54 | obj_new.cls_num = data.object_list[ii].cls 55 | obj_new.height_factor = data.object_list[ii].height_factor 56 | obj_new.object_height = data.object_list[ii].object_height 57 | new_center = np.array([[data.object_list[ii].x_center,data.object_list[ii].y_center]]) 58 | 59 | 60 | # The first object initializer: 61 | if len(object_class_list) == 0: 62 | print ('Adding first object.') 63 | # Adding the first object to the map msg: 64 | object_mapped_values.object_map.append(obj_new) 65 | OM_publisher.publish(object_mapped_values) 66 | # Adding the first object to the map class: 67 | object_class = Object_Map_cls() 68 | object_class.x_center = obj_new.x_center 69 | object_class.y_center = obj_new.y_center 70 | object_class.r = obj_new.r 71 | object_class.a = obj_new.a 72 | object_class.b = obj_new.b 73 | object_class.angle = obj_new.angle 74 | object_class.probabilities = obj_new.probabilities 75 | object_class.cls_num = obj_new.cls_num 76 | object_class_list.append(object_class) 77 | # Adding the first center to numpy array: 78 | objects_center = np.array([[object_class.x_center,object_class.y_center]]) 79 | print ('Added first object.') 80 | continue 81 | 82 | 83 | dist,index = closest_node(new_center,objects_center) 84 | 85 | env = Search_Radius(3*object_class_list[index].r,object_class_list[index].a,object_class_list[index].b) 86 | 87 | if index in A_Elliptical: 88 | env = env * 4 89 | elif index in A_Rectangle: 90 | env = env * 1.5 91 | 92 | if dist > env: 93 | print ('Adding a new object.') 94 | # Adding the object to the map msg: 95 | object_mapped_values.object_map.append(obj_new) 96 | OM_publisher.publish(object_mapped_values) 97 | # Adding the object to the map class: 98 | object_class = Object_Map_cls() 99 | object_class.x_center = obj_new.x_center 100 | object_class.y_center = obj_new.y_center 101 | object_class.r = obj_new.r 102 | object_class.a = obj_new.a 103 | object_class.b = obj_new.b 104 | object_class.angle = obj_new.angle 105 | object_class.probabilities = obj_new.probabilities 106 | object_class.cls_num = obj_new.cls_num 107 | object_class_list.append(object_class) 108 | # Adding the center to numpy array: 109 | objects_center = np.concatenate((objects_center,new_center)) 110 | print ('Added a new object.') 111 | 112 | else: 113 | print ('Start to update.') 114 | object_class_list[index].prob_distribution ,object_class_list[index].cls_num = Updated_Probabilities_and_Cls(object_class_list[index].prob_distribution,obj_new.probabilities,obj_new.cls_num) 115 | 116 | object_mapped_values.object_map[index].probabilities = object_class_list[index].prob_distribution 117 | 118 | object_mapped_values.object_map[index].cls_num = np.int16(object_class_list[index].cls_num) 119 | 120 | # Updating theta: 121 | [x,y,r,a,b,phi] = Theta_updater(obj_new.x_center,object_class_list[index].x_center, 122 | obj_new.y_center,object_class_list[index].y_center, 123 | obj_new.r,object_class_list[index].r, 124 | obj_new.a,object_class_list[index].a, 125 | obj_new.b,object_class_list[index].b, 126 | obj_new.angle,object_class_list[index].angle) 127 | 128 | object_class_list[index].x_center = x 129 | object_class_list[index].y_center = y 130 | object_class_list[index].r = r 131 | object_class_list[index].a = a 132 | object_class_list[index].b = b 133 | object_class_list[index].angle = phi 134 | 135 | object_mapped_values.object_map[index].x_center = x 136 | object_mapped_values.object_map[index].y_center = y 137 | object_mapped_values.object_map[index].r = r 138 | object_mapped_values.object_map[index].a = a 139 | object_mapped_values.object_map[index].b = b 140 | object_mapped_values.object_map[index].angle = phi 141 | 142 | 143 | print ('Updated an existed object.') 144 | 145 | OM_publisher.publish(object_mapped_values) 146 | #rr.sleep() 147 | -------------------------------------------------------------------------------- /object_mapping/object_mapper/Publish_object_to_rviz.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Libraries: 4 | import rospy 5 | from tf.transformations import quaternion_from_euler 6 | import numpy as np 7 | 8 | # msgs: 9 | 10 | from object_mapping.msg import Object_Map 11 | from visualization_msgs.msg import Marker , MarkerArray 12 | 13 | def callback_map(data): 14 | global O_map 15 | O_map = data 16 | 17 | 18 | rospy.init_node('Points_to_rvis', anonymous=True) 19 | 20 | # Publisher: 21 | O_publisher = rospy.Publisher('/Objects' , MarkerArray , queue_size = 5) 22 | 23 | #Subsctibers: 24 | M_Subs = rospy.Subscriber('/object_mapped_values',Object_Map,callback_map) 25 | rospy.wait_for_message('/object_mapped_values',Object_Map) 26 | 27 | global O_map 28 | 29 | A_Round = rospy.get_param('/Array/round') 30 | A_Rectangle = rospy.get_param('/Array/rectangle') 31 | A_Elliptical = rospy.get_param('/Array/elliptical') 32 | 33 | while not rospy.is_shutdown(): 34 | M_data = O_map 35 | list_marker = MarkerArray() 36 | list_marker.markers = [] 37 | 38 | 39 | for ii in range(len(M_data.object_map)): 40 | marker = Marker() 41 | marker_name = Marker() 42 | # For the angle: 43 | [x_q,y_q,z_q,w_q] = quaternion_from_euler(0,0,M_data.object_map[ii].angle) 44 | 45 | name = rospy.get_param('/object_list/o'+str(M_data.object_map[ii].cls_num)+'/name') 46 | 47 | height_factor = M_data.object_map[ii].height_factor 48 | 49 | if height_factor == 0: 50 | height_factor = 1 51 | 52 | if M_data.object_map[ii].cls_num in A_Round: 53 | 54 | # Test for putting a can in the map. 55 | marker.header.frame_id = 'map' 56 | marker.header.stamp = rospy.Time.now() 57 | # Colore of can 58 | marker.color.r = 0 59 | marker.color.g = 0 60 | marker.color.b = 200 61 | marker.color.a = 1 62 | # Rest of things: 63 | marker.id = 1 64 | marker.type = 3 65 | marker.action = 0 66 | marker.lifetime.secs = 0 67 | # Orientation of the can 68 | marker.pose.orientation.x = 0 69 | marker.pose.orientation.y = 0 70 | marker.pose.orientation.z = 0 71 | marker.pose.orientation.w = 1 72 | # Size: 73 | marker.scale.x = 2 * M_data.object_map[ii].r 74 | marker.scale.y = 2 * M_data.object_map[ii].r 75 | marker.scale.z = height_factor * marker.scale.x 76 | # Location of the can: 77 | marker.pose.position.x = M_data.object_map[ii].x_center 78 | marker.pose.position.y = M_data.object_map[ii].y_center 79 | #marker.pose.position.z = marker.scale.z/2 80 | marker.scale.z = M_data.object_map[ii].object_height 81 | # Name: 82 | marker.ns = name + str(ii) 83 | list_marker.markers.append(marker) 84 | 85 | # Text adding: 86 | marker_name.header.frame_id = 'map' 87 | marker_name.header.stamp = rospy.Time.now() 88 | # Orientation of the text 89 | marker_name.pose.orientation.x = 0 90 | marker_name.pose.orientation.y = 0 91 | marker_name.pose.orientation.z = 0 92 | marker_name.pose.orientation.w = 1 93 | # Colore of text 94 | marker_name.color.r = 0 95 | marker_name.color.g = 0 96 | marker_name.color.b = 0 97 | marker_name.color.a = 1 98 | # Rest of things: 99 | marker_name.id = 10 100 | marker_name.type = 9 101 | marker_name.action = 0 102 | marker_name.lifetime.secs = 0 103 | marker_name.pose.position.x = M_data.object_map[ii].x_center 104 | marker_name.pose.position.y = M_data.object_map[ii].y_center 105 | marker_name.pose.position.z = 2*marker.pose.position.z + 0.1 106 | # Size of the text 107 | marker_name.scale.x = 0.4 108 | marker_name.scale.y = 0.4 109 | marker_name.scale.z = 0.4 110 | marker_name.text = name 111 | marker_name.ns = name + str(ii) 112 | 113 | list_marker.markers.append(marker_name) 114 | continue 115 | 116 | 117 | 118 | elif M_data.object_map[ii].cls_num in A_Rectangle: 119 | # Test for putting a TV in the map. 120 | marker.header.frame_id = 'map' 121 | marker.header.stamp = rospy.Time.now() 122 | # Colour of TV 123 | marker.color.r = 100 124 | marker.color.g = 0 125 | marker.color.b = 100 126 | marker.color.a = 1 127 | # Rest of things: 128 | marker.id = 1 129 | marker.type = 1 130 | marker.action = 0 131 | marker.lifetime.secs = 0 132 | # Orientation of the TV 133 | marker.pose.orientation.x = x_q 134 | marker.pose.orientation.y = y_q 135 | marker.pose.orientation.z = z_q 136 | marker.pose.orientation.w = w_q 137 | # Size: 138 | marker.scale.x = M_data.object_map[ii].a 139 | marker.scale.y = M_data.object_map[ii].b 140 | angle_O = M_data.object_map[ii].angle 141 | #marker.scale.z = np.absolute(height_factor * marker.scale.x * np.sin(angle_O)) 142 | marker.scale.z = M_data.object_map[ii].object_height 143 | 144 | # Location of the TV: 145 | marker.pose.position.x = M_data.object_map[ii].x_center 146 | marker.pose.position.y = M_data.object_map[ii].y_center 147 | marker.pose.position.z = marker.scale.z/2 148 | # Name: 149 | marker.ns = name + str(ii) 150 | list_marker.markers.append(marker) 151 | 152 | # Text adding: 153 | marker_name.header.frame_id = 'map' 154 | marker_name.header.stamp = rospy.Time.now() 155 | # Orientation of the text 156 | marker_name.pose.orientation.x = x_q 157 | marker_name.pose.orientation.y = y_q 158 | marker_name.pose.orientation.z = z_q 159 | marker_name.pose.orientation.w = w_q 160 | # Colour of text 161 | marker_name.color.r = 0 162 | marker_name.color.g = 0 163 | marker_name.color.b = 0 164 | marker_name.color.a = 1 165 | # Rest of things: 166 | marker_name.id = 10 167 | marker_name.type = 9 168 | marker_name.action = 0 169 | marker_name.lifetime.secs = 0 170 | marker_name.pose.position.x = M_data.object_map[ii].x_center 171 | marker_name.pose.position.y = M_data.object_map[ii].y_center 172 | marker_name.pose.position.z = 2*marker.pose.position.z + 0.1 173 | # Size of the text 174 | marker_name.scale.x = 0.4 175 | marker_name.scale.y = 0.4 176 | marker_name.scale.z = 0.4 177 | marker_name.text = name 178 | marker_name.ns = name + str(ii) 179 | 180 | list_marker.markers.append(marker_name) 181 | continue 182 | 183 | else: 184 | # Test for putting an elliptical in the map. 185 | marker.header.frame_id = 'map' 186 | marker.header.stamp = rospy.Time.now() 187 | 188 | # Rest of things: 189 | marker.id = 1 190 | marker.type = 3 191 | marker.action = 0 192 | marker.lifetime.secs = 0 193 | # Orientation of the TV 194 | marker.pose.orientation.x = x_q 195 | marker.pose.orientation.y = y_q 196 | marker.pose.orientation.z = z_q 197 | marker.pose.orientation.w = w_q 198 | # Size: 199 | marker.scale.x = 2*M_data.object_map[ii].a 200 | marker.scale.y = 2*M_data.object_map[ii].b 201 | 202 | #angle_O = M_data.object_map[ii].angle 203 | #marker.scale.z = np.absolute(height_factor * marker.scale.x * np.sin(angle_O)) 204 | marker.scale.z = M_data.object_map[ii].object_height 205 | 206 | if M_data.object_map[ii].cls_num == 15: 207 | marker.scale.z *= 1.4 208 | # Location of the TV: 209 | marker.pose.position.x = M_data.object_map[ii].x_center 210 | marker.pose.position.y = M_data.object_map[ii].y_center 211 | marker.pose.position.z = marker.scale.z/2 212 | 213 | 214 | marker.color.r = 50 215 | marker.color.g = 50 216 | marker.color.b = 100 217 | marker.color.a = 1 218 | marker.ns = name + str(ii) 219 | marker_name.text = name 220 | marker_name.ns = name + str(ii) 221 | 222 | 223 | list_marker.markers.append(marker) 224 | 225 | # Text adding: 226 | marker_name.header.frame_id = 'map' 227 | marker_name.header.stamp = rospy.Time.now() 228 | # Orientation of the text 229 | marker_name.pose.orientation.x = x_q 230 | marker_name.pose.orientation.y = y_q 231 | marker_name.pose.orientation.z = z_q 232 | marker_name.pose.orientation.w = w_q 233 | # Colour of text 234 | marker_name.color.r = 0 235 | marker_name.color.g = 0 236 | marker_name.color.b = 0 237 | marker_name.color.a = 1 238 | # Rest of things: 239 | marker_name.id = 10 240 | marker_name.type = 9 241 | marker_name.action = 0 242 | marker_name.lifetime.secs = 0 243 | marker_name.pose.position.x = M_data.object_map[ii].x_center 244 | marker_name.pose.position.y = M_data.object_map[ii].y_center 245 | marker_name.pose.position.z = 2*marker.pose.position.z + 0.1 246 | # Size of the text 247 | marker_name.scale.x = 0.4 248 | marker_name.scale.y = 0.4 249 | marker_name.scale.z = 0.4 250 | 251 | list_marker.markers.append(marker_name) 252 | continue 253 | 254 | #publish: 255 | O_publisher.publish(list_marker) 256 | for jj in range(len(list_marker.markers)): 257 | list_marker.markers[jj].action = 2 258 | 259 | 260 | -------------------------------------------------------------------------------- /object_mapping/object_mapper/Results_code/Results_table_excel.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import xlsxwriter 5 | 6 | from object_mapping.msg import M 7 | 8 | def callback_map(data): 9 | global O_map 10 | O_map = data 11 | exp_num = input ('Experiment number: ') 12 | num = input ('Round number: ') 13 | 14 | if num == 1: 15 | workbook_name = 'new_Results' + str(exp_num) + '.xlsx' 16 | else: 17 | workbook_name = 'new_Results' + str(exp_num) + '_second_round.xlsx' 18 | workbook = xlsxwriter.Workbook(workbook_name) 19 | worksheet = workbook.add_worksheet() 20 | row = 0 21 | column = 0 22 | 23 | for ii in range(0,len(O_map.M)): 24 | 25 | names = [str(ii), 26 | 'bicycle','bird','bottle','cat','Chair', 27 | 'dog','motorbike','person','pottedplant', 28 | 'sheep','sofa','tvmonitor'] 29 | 30 | 31 | x = [] 32 | y = [] 33 | probabilities = [] 34 | x_max = 0 35 | y_max = 0 36 | prob_max = 0 37 | 38 | for jj in range(0,len(O_map.M[ii].m_i)): 39 | x.append(O_map.M[ii].m_i[jj].x_center) 40 | y.append(O_map.M[ii].m_i[jj].y_center) 41 | probabilities.append(O_map.M[ii].m_i[jj].probability) 42 | if O_map.M[ii].m_i[jj].probability > prob_max: 43 | x_max= O_map.M[ii].m_i[jj].x_center 44 | y_max = O_map.M[ii].m_i[jj].y_center 45 | prob_max= O_map.M[ii].m_i[jj].probability 46 | name_max = names[jj+1] 47 | 48 | for name in names: 49 | 50 | worksheet.write(row, column, name) 51 | column += 1 52 | column += 2 53 | worksheet.write(row, column, name_max) 54 | row += 1 55 | column = 0 56 | 57 | worksheet.write(row, column, 'x') 58 | column += 1 59 | for num in x: 60 | worksheet.write(row, column, num) 61 | column += 1 62 | column += 2 63 | worksheet.write(row, column, x_max) 64 | row += 1 65 | column = 0 66 | 67 | worksheet.write(row, column, 'y') 68 | column += 1 69 | for num in y: 70 | worksheet.write(row, column, num) 71 | column += 1 72 | column += 2 73 | worksheet.write(row, column, y_max) 74 | row += 1 75 | column = 0 76 | 77 | worksheet.write(row, column, 'prob') 78 | column += 1 79 | for num in probabilities: 80 | worksheet.write(row, column, num) 81 | column += 1 82 | column += 2 83 | worksheet.write(row, column, prob_max) 84 | row += 2 85 | column = 0 86 | 87 | 88 | workbook.close() 89 | 90 | rospy.init_node('xls_Objects_node', anonymous=True) 91 | 92 | 93 | #Subsctibers: 94 | M_Subs = rospy.Subscriber('/M',M,callback_map) 95 | rospy.wait_for_message('/M',M) 96 | 97 | 98 | -------------------------------------------------------------------------------- /object_mapping/object_mapper/Results_code/TV_xls.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | 5 | import xlsxwriter 6 | 7 | from object_mapping.msg import M 8 | 9 | global number_of_test 10 | 11 | 12 | def callback_map(data): 13 | global O_map 14 | O_map = data 15 | global number_of_test 16 | 17 | 18 | 19 | workbook = xlsxwriter.Workbook('TV10.xlsx') 20 | worksheet = workbook.add_worksheet() 21 | row = 0 22 | column = 0 23 | 24 | for ii in range(0,len(O_map.M)): 25 | 26 | names = ['obj','TV'] 27 | for name in names: 28 | 29 | worksheet.write(row, column, name) 30 | column += 1 31 | row += 1 32 | column = 0 33 | 34 | x = [] 35 | y = [] 36 | probabilities = [] 37 | 38 | for jj in range(0,len(O_map.M[ii].m_i)): 39 | if O_map.M[ii].m_i[jj].cls_num == 20: 40 | x.append(O_map.M[ii].m_i[jj].x_center) 41 | y.append(O_map.M[ii].m_i[jj].y_center) 42 | probabilities.append(O_map.M[ii].m_i[jj].probability) 43 | 44 | worksheet.write(row, column, 'x') 45 | column += 1 46 | for num in x: 47 | worksheet.write(row, column, num) 48 | column += 1 49 | row += 1 50 | column = 0 51 | 52 | worksheet.write(row, column, 'y') 53 | column += 1 54 | 55 | for num in y: 56 | worksheet.write(row, column, num) 57 | column += 1 58 | row += 1 59 | column = 0 60 | 61 | worksheet.write(row, column, 'prob') 62 | column += 1 63 | for num in probabilities: 64 | worksheet.write(row, column, num) 65 | column += 1 66 | row += 2 67 | column = 0 68 | 69 | workbook.close() 70 | 71 | rospy.init_node('xls_TV_node', anonymous=True) 72 | 73 | 74 | #Subsctibers: 75 | M_Subs = rospy.Subscriber('/M',M,callback_map) 76 | rospy.wait_for_message('/M',M) 77 | 78 | 79 | -------------------------------------------------------------------------------- /object_mapping/object_mapper/Results_code/dog_xls.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | 5 | import xlsxwriter 6 | 7 | from object_mapping.msg import M 8 | 9 | def callback_map(data): 10 | global O_map 11 | O_map = data 12 | workbook = xlsxwriter.Workbook('dog11.xlsx') 13 | worksheet = workbook.add_worksheet() 14 | row = 0 15 | column = 0 16 | 17 | for ii in range(0,len(O_map.M)): 18 | 19 | names = ['obj','dog'] 20 | for name in names: 21 | 22 | worksheet.write(row, column, name) 23 | column += 1 24 | row += 1 25 | column = 0 26 | 27 | x = [] 28 | y = [] 29 | probabilities = [] 30 | 31 | for jj in range(0,len(O_map.M[ii].m_i)): 32 | if O_map.M[ii].m_i[jj].cls_num == 12: 33 | x.append(O_map.M[ii].m_i[jj].x_center) 34 | y.append(O_map.M[ii].m_i[jj].y_center) 35 | probabilities.append(O_map.M[ii].m_i[jj].probability) 36 | 37 | worksheet.write(row, column, 'x') 38 | column += 1 39 | for num in x: 40 | worksheet.write(row, column, num) 41 | column += 1 42 | row += 1 43 | column = 0 44 | 45 | worksheet.write(row, column, 'y') 46 | column += 1 47 | 48 | for num in y: 49 | worksheet.write(row, column, num) 50 | column += 1 51 | row += 1 52 | column = 0 53 | 54 | worksheet.write(row, column, 'prob') 55 | column += 1 56 | for num in probabilities: 57 | worksheet.write(row, column, num) 58 | column += 1 59 | row += 2 60 | column = 0 61 | 62 | workbook.close() 63 | 64 | rospy.init_node('xls_dog_node', anonymous=True) 65 | 66 | 67 | #Subsctibers: 68 | M_Subs = rospy.Subscriber('/M',M,callback_map) 69 | rospy.wait_for_message('/M',M) 70 | 71 | 72 | -------------------------------------------------------------------------------- /object_mapping/object_mapper/Results_code/making_results.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import numpy as np 4 | import xlsxwriter 5 | 6 | from object_mapping.msg import M 7 | from gazebo_msgs.msg import ModelStates 8 | 9 | global x_model , y_model , name 10 | 11 | 12 | def callback_model(data): 13 | 14 | global x_model , y_model , name 15 | name = data.name 16 | x_model = [] 17 | y_model = [] 18 | for ii in range(0,len(name)): 19 | x_model.append(data.pose[ii].position.x) 20 | y_model.append(data.pose[ii].position.y) 21 | x_model = np.array(x_model) 22 | y_model = np.array(y_model) 23 | 24 | rospy.init_node('xls_MObjects_node', anonymous=True) 25 | 26 | M_sub = rospy.Subscriber('/gazebo/model_states',ModelStates,callback_model) 27 | rospy.wait_for_message('/gazebo/model_states',ModelStates) 28 | 29 | 30 | print "Start Model_table xls!" 31 | workbook = xlsxwriter.Workbook('Model_table_smaller_eps.xlsx') 32 | worksheet = workbook.add_worksheet() 33 | row = 0 34 | column = 0 35 | 36 | worksheet.write(row, column, 'model_name') 37 | column += 1 38 | 39 | for n in name: 40 | 41 | worksheet.write(row, column, n) 42 | column += 1 43 | row += 1 44 | column = 0 45 | 46 | worksheet.write(row, column, 'x') 47 | column += 1 48 | for num in x_model: 49 | worksheet.write(row, column, num) 50 | column += 1 51 | row += 1 52 | column = 0 53 | 54 | worksheet.write(row, column, 'y') 55 | column += 1 56 | 57 | for num in y_model: 58 | worksheet.write(row, column, num) 59 | column += 1 60 | row += 1 61 | column = 0 62 | 63 | 64 | 65 | workbook.close() 66 | 67 | print "Done Model_table xls!" -------------------------------------------------------------------------------- /object_mapping/object_mapper/Results_code/map2csv.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import numpy as np 5 | from object_mapping.msg import Object_Map 6 | import matplotlib.pyplot as plt 7 | import pandas as pd 8 | 9 | 10 | def Map_callback(data): 11 | global Map_sub 12 | Map_sub = data 13 | 14 | 15 | rospy.init_node('LaserMatrix', anonymous=True) 16 | subscriber = rospy.Subscriber('/object_mapped_values',Object_Map, Map_callback) 17 | rospy.wait_for_message('/object_mapped_values',Object_Map) 18 | 19 | 20 | def M2C(): 21 | global Map_sub 22 | Map = Map_sub.object_map 23 | df = pd.DataFrame(Map) 24 | df.to_csv('old_code_round_3.csv') 25 | 26 | if __name__ == '__main__': 27 | 28 | #Testing our function 29 | M2C() -------------------------------------------------------------------------------- /object_mapping/object_mapper/Updated_object_mapping.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Libraries: 4 | import rospy 5 | import numpy as np 6 | from H_functions import SO_class, q_i , making_M , Prob_updater 7 | import pandas as pd 8 | 9 | # Msgs: 10 | from joint_object_localizer.msg import M_Suggested_List 11 | from object_mapping.msg import Single_Class , M_i , M 12 | 13 | global last_location_x , last_location_y 14 | last_location_x = 0 15 | last_location_y = 0 16 | 17 | def M_o_callback(data): 18 | 19 | global epsilon 20 | global M_list_class 21 | global last_location_x , last_location_y 22 | # The subs: 23 | global Theta_list 24 | Theta_list = data 25 | 26 | x = [] 27 | y = [] 28 | r = [] 29 | a = [] 30 | b = [] 31 | angle = [] 32 | cls_num = [] 33 | Prb = [] 34 | object_height = [] 35 | 36 | # Updating the msg: 37 | for jj in range(0,len(data.M_list)): 38 | 39 | # Arranging the data for Mo: 40 | for i in range(0,len(data.M_list[jj].object_list)): 41 | 42 | # getting the fresh data 43 | object_height.append(data.M_list[jj].object_list[i].object_height) 44 | x.append(data.M_list[jj].object_list[i].x_center) 45 | y.append(data.M_list[jj].object_list[i].y_center) 46 | r.append(data.M_list[jj].object_list[i].r) 47 | a.append(data.M_list[jj].object_list[i].a) 48 | b.append(data.M_list[jj].object_list[i].b) 49 | angle.append(data.M_list[jj].object_list[i].angle) 50 | cls_num.append(data.M_list[jj].object_list[i].cls) 51 | Prb.append(data.M_list[jj].object_list[i].Final_Likelihood) 52 | 53 | # In case the same object has been sent more than once: 54 | if x[0] == last_location_x and y[0] == last_location_y: 55 | return 0 56 | 57 | last_location_x = x[0] 58 | last_location_y = y[0] 59 | 60 | # The new object: 61 | Mo = SO_class(x_center=x,y_center=y,r=r,a=a,b=b,angle=angle, 62 | cls_num=cls_num,prob_distribution=Prb,object_height=object_height) 63 | 64 | print ('Got new Mo') 65 | if len(M_list_class) == 0: 66 | print ("First Object") 67 | M_list_class.append(Mo) 68 | 69 | 70 | else: 71 | q = [] 72 | for ii in range(0,len(M_list_class)): 73 | q.append(q_i(M_list_class[ii],Mo)) 74 | 75 | ii = np.argmax(q) 76 | 77 | # New object 78 | if q[ii] < epsilon: 79 | print ('Added an object') 80 | M_list_class.append(Mo) 81 | 82 | # Updating an object: 83 | else: 84 | print ('Updating an object') 85 | alpha = 0.5 86 | 87 | # Updating: 88 | M_list_class[ii].prob_distribution = Prob_updater(alpha, 89 | M_list_class[ii].prob_distribution, 90 | Mo.prob_distribution) 91 | 92 | M_list_class[ii].x_center = alpha * M_list_class[ii].x_center\ 93 | + (1-alpha)*Mo.x_center 94 | M_list_class[ii].y_center = alpha * M_list_class[ii].y_center\ 95 | + (1-alpha)*Mo.y_center 96 | M_list_class[ii].r = alpha * M_list_class[ii].r + (1-alpha)*Mo.r 97 | M_list_class[ii].a = alpha * M_list_class[ii].a + (1-alpha)*Mo.a 98 | M_list_class[ii].b = alpha * M_list_class[ii].b + (1-alpha)*Mo.b 99 | M_list_class[ii].angle = alpha * M_list_class[ii].angle + (1-alpha)*Mo.angle 100 | M_list_class[ii].object_height = alpha * M_list_class[ii].object_height + (1-alpha)*Mo.object_height 101 | 102 | M_msg = making_M(M_list_class) 103 | OM_publisher.publish(M_msg) 104 | if len(data.M_list)>0: 105 | print ("Done") 106 | 107 | 108 | # Initial node: 109 | rospy.init_node('List_of_mapped_objects', anonymous=True) 110 | 111 | 112 | global M_list_class 113 | global epsilon 114 | epsilon = rospy.get_param('/Cov/epsilon') 115 | M_list_class = [] 116 | # Publishers: 117 | OM_publisher = rospy.Publisher('/M',M,queue_size=5) 118 | 119 | while not rospy.is_shutdown(): 120 | 121 | Theta_list_sub = rospy.Subscriber('/M_o',M_Suggested_List,M_o_callback,queue_size=20) 122 | rospy.wait_for_message('/M_o',M_Suggested_List) 123 | 124 | 125 | 126 | 127 | 128 | -------------------------------------------------------------------------------- /object_mapping/object_mapper/confusion_matrix_corrected.csv: -------------------------------------------------------------------------------- 1 | ,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19 2 | 0,0.76,0.0,0.0,0.1,0.0,0.0,0.01,0.01,0.01,0.01,0.01,0.0,0.01,0.01,0.04,0.0,0.02,0.0,0.0,0.01 3 | 1,0.01,0.78,0.01,0.11,0.01,0.0,0.01,0.01,0.01,0.0,0.0,0.0,0.0,0.0,0.02,0.0,0.01,0.0,0.01,0.01 4 | 2,0.01,0.0,0.77,0.1,0.0,0.0,0.01,0.0,0.01,0.01,0.01,0.01,0.01,0.0,0.02,0.01,0.01,0.0,0.01,0.01 5 | 3,0.00980392156862745,0.0,0.11764705882352941,0.7058823529411764,0.00980392156862745,0.0,0.00980392156862745,0.00980392156862745,0.00980392156862745,0.00980392156862745,0.00980392156862745,0.00980392156862745,0.0196078431372549,0.00980392156862745,0.0392156862745098,0.0,0.0196078431372549,0.0,0.0,0.00980392156862745 6 | 4,0.01020408163265306,0.0,0.12244897959183672,0.01020408163265306,0.7448979591836734,0.0,0.0,0.0,0.01020408163265306,0.01020408163265306,0.01020408163265306,0.01020408163265306,0.01020408163265306,0.0,0.03061224489795918,0.0,0.03061224489795918,0.0,0.0,0.0 7 | 5,0.0,0.01,0.09,0.0,0.0,0.79,0.01,0.01,0.0,0.0,0.01,0.01,0.0,0.0,0.04,0.0,0.02,0.0,0.0,0.01 8 | 6,0.009900990099009901,0.009900990099009901,0.07920792079207921,0.009900990099009901,0.0,0.009900990099009901,0.7623762376237624,0.009900990099009901,0.009900990099009901,0.009900990099009901,0.009900990099009901,0.009900990099009901,0.009900990099009901,0.009900990099009901,0.009900990099009901,0.009900990099009901,0.019801980198019802,0.0,0.0,0.009900990099009901 9 | 7,0.02040816326530612,0.0,0.10204081632653061,0.0,0.0,0.01020408163265306,0.0,0.826530612244898,0.0,0.0,0.0,0.01020408163265306,0.0,0.0,0.0,0.01020408163265306,0.01020408163265306,0.0,0.01020408163265306,0.0 10 | 8,0.01,0.0,0.09,0.01,0.0,0.01,0.0,0.01,0.78,0.01,0.01,0.01,0.0,0.01,0.01,0.03,0.01,0.0,0.0,0.0 11 | 9,0.0,0.0,0.0,0.10204081632653061,0.01020408163265306,0.0,0.01020408163265306,0.0,0.0,0.8061224489795918,0.0,0.0,0.0,0.0,0.01020408163265306,0.0,0.02040816326530612,0.01020408163265306,0.02040816326530612,0.01020408163265306 12 | 10,0.0,0.010309278350515464,0.0,0.12371134020618556,0.0,0.0,0.010309278350515464,0.010309278350515464,0.0,0.0,0.7938144329896907,0.0,0.0,0.0,0.0,0.0,0.020618556701030927,0.010309278350515464,0.010309278350515464,0.010309278350515464 13 | 11,0.009900990099009901,0.009900990099009901,0.009900990099009901,0.10891089108910891,0.009900990099009901,0.0,0.009900990099009901,0.009900990099009901,0.009900990099009901,0.0,0.0,0.7524752475247525,0.009900990099009901,0.0,0.0,0.0,0.019801980198019802,0.019801980198019802,0.009900990099009901,0.009900990099009901 14 | 12,0.0,0.01,0.01,0.09,0.01,0.0,0.01,0.01,0.0,0.0,0.0,0.01,0.8,0.0,0.0,0.0,0.02,0.01,0.01,0.01 15 | 13,0.0,0.01020408163265306,0.01020408163265306,0.09183673469387754,0.01020408163265306,0.0,0.01020408163265306,0.01020408163265306,0.0,0.0,0.01020408163265306,0.0,0.0,0.8163265306122449,0.0,0.0,0.02040816326530612,0.0,0.01020408163265306,0.0 16 | 14,0.0,0.009900990099009901,0.009900990099009901,0.009900990099009901,0.009900990099009901,0.0,0.009900990099009901,0.009900990099009901,0.009900990099009901,0.0,0.009900990099009901,0.009900990099009901,0.009900990099009901,0.009900990099009901,0.8316831683168316,0.009900990099009901,0.019801980198019802,0.009900990099009901,0.009900990099009901,0.009900990099009901 17 | 15,0.0,0.01,0.0,0.01,0.12,0.0,0.0,0.0,0.01,0.01,0.01,0.01,0.0,0.01,0.0,0.76,0.02,0.01,0.02,0.0 18 | 16,0.0,0.0,0.0,0.0,0.1212121212121212,0.0101010101010101,0.0,0.0,0.0101010101010101,0.0101010101010101,0.0,0.0101010101010101,0.0101010101010101,0.0101010101010101,0.0,0.0202020202020202,0.7676767676767676,0.0101010101010101,0.0202020202020202,0.0 19 | 17,0.0,0.010309278350515464,0.010309278350515464,0.0,0.10309278350515463,0.0,0.0,0.0,0.0,0.0,0.0,0.010309278350515464,0.0,0.0,0.0,0.020618556701030927,0.010309278350515464,0.8144329896907216,0.010309278350515464,0.010309278350515464 20 | 18,0.0,0.01,0.01,0.01,0.08,0.0,0.0,0.01,0.0,0.0,0.01,0.0,0.0,0.0,0.01,0.03,0.01,0.01,0.81,0.0 21 | 19,0.0,0.010204081632653062,0.0,0.0,0.11224489795918367,0.010204081632653062,0.0,0.010204081632653062,0.0,0.0,0.0,0.010204081632653062,0.010204081632653062,0.010204081632653062,0.0,0.030612244897959183,0.0,0.020408163265306124,0.0,0.7755102040816326 22 | -------------------------------------------------------------------------------- /object_mapping/object_mapper/map2rviz.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Libraries: 4 | import rospy 5 | from tf.transformations import quaternion_from_euler 6 | import numpy as np 7 | 8 | # msgs: 9 | from object_mapping.msg import M 10 | from visualization_msgs.msg import Marker , MarkerArray 11 | 12 | def callback_map(data): 13 | global O_map 14 | O_map = data 15 | 16 | 17 | rospy.init_node('Rviz_Objects_node', anonymous=True) 18 | 19 | # Publisher: 20 | O_publisher = rospy.Publisher('/Rviz_Objects' , MarkerArray , queue_size = 10) 21 | 22 | #Subsctibers: 23 | M_Subs = rospy.Subscriber('/M',M,callback_map) 24 | rospy.wait_for_message('/M',M) 25 | 26 | global O_map 27 | 28 | A_Round = rospy.get_param('/Array/round') 29 | A_Rectangle = rospy.get_param('/Array/rectangle') 30 | A_Elliptical = rospy.get_param('/Array/elliptical') 31 | 32 | slp = rospy.Rate(5) 33 | 34 | while not rospy.is_shutdown(): 35 | 36 | M_data = O_map 37 | slp.sleep() 38 | list_marker = MarkerArray() 39 | list_marker.markers = [] 40 | 41 | 42 | for ii in range(len(M_data.M)): 43 | marker = Marker() 44 | marker_name = Marker() 45 | probability = [] 46 | for rr in range (0,len(M_data.M[ii].m_i)): 47 | probability.append(M_data.M[ii].m_i[rr].probability) 48 | max_p = np.argmax(np.array(probability)) 49 | # For the angle: 50 | [x_q,y_q,z_q,w_q] = quaternion_from_euler(0,0,M_data.M[ii].m_i[max_p].angle) 51 | 52 | name = rospy.get_param('/object_list/o'+str(M_data.M[ii].m_i[max_p].cls_num)+'/name') 53 | 54 | 55 | object_height = M_data.M[ii].m_i[max_p].object_height 56 | if M_data.M[ii].m_i[max_p].cls_num in A_Round: 57 | 58 | # Test for putting a can in the map. 59 | marker.header.frame_id = 'map' 60 | marker.header.stamp = rospy.Time.now() 61 | # Colore of can 62 | marker.color.r = 0 63 | marker.color.g = 0 64 | marker.color.b = 200 65 | marker.color.a = 1 66 | # Rest of things: 67 | marker.id = 1 68 | marker.type = 3 69 | marker.action = 0 70 | marker.lifetime.secs = 0 71 | # Orientation of the can 72 | marker.pose.orientation.x = 0 73 | marker.pose.orientation.y = 0 74 | marker.pose.orientation.z = 0 75 | marker.pose.orientation.w = 1 76 | # Size: 77 | marker.scale.x = 2 * M_data.M[ii].m_i[max_p].r 78 | marker.scale.y = 2 * M_data.M[ii].m_i[max_p].r 79 | marker.scale.z = object_height 80 | # Location of the can: 81 | marker.pose.position.x = M_data.M[ii].m_i[max_p].x_center 82 | marker.pose.position.y = M_data.M[ii].m_i[max_p].y_center 83 | marker.pose.position.z = marker.scale.z/2 84 | # Name: 85 | marker.ns = name + str(ii) 86 | list_marker.markers.append(marker) 87 | 88 | # Text adding: 89 | marker_name.header.frame_id = 'map' 90 | marker_name.header.stamp = rospy.Time.now() 91 | # Orientation of the text 92 | marker_name.pose.orientation.x = 0 93 | marker_name.pose.orientation.y = 0 94 | marker_name.pose.orientation.z = 0 95 | marker_name.pose.orientation.w = 1 96 | # Colore of text 97 | marker_name.color.r = 200 98 | marker_name.color.g = 200 99 | marker_name.color.b = 200 100 | marker_name.color.a = 1 101 | # Rest of things: 102 | marker_name.id = 10 103 | marker_name.type = 9 104 | marker_name.action = 0 105 | marker_name.lifetime.secs = 0 106 | marker_name.pose.position.x = M_data.M[ii].m_i[max_p].x_center 107 | marker_name.pose.position.y = M_data.M[ii].m_i[max_p].y_center 108 | marker_name.pose.position.z = 2*marker.pose.position.z + 0.1 109 | # Size of the text 110 | marker_name.scale.x = 0.2 111 | marker_name.scale.y = 0.2 112 | marker_name.scale.z = 0.2 113 | marker_name.text = name 114 | marker_name.ns = name + str(ii) 115 | 116 | list_marker.markers.append(marker_name) 117 | continue 118 | 119 | 120 | 121 | if M_data.M[ii].m_i[max_p].cls_num in A_Rectangle: 122 | # Test for putting a TV in the map. 123 | marker.header.frame_id = 'map' 124 | marker.header.stamp = rospy.Time.now() 125 | # Colour of TV 126 | marker.color.r = 100 127 | marker.color.g = 0 128 | marker.color.b = 100 129 | marker.color.a = 1 130 | # Rest of things: 131 | marker.id = 1 132 | marker.type = 1 133 | marker.action = 0 134 | marker.lifetime.secs = 0 135 | # Orientation of the TV 136 | marker.pose.orientation.x = x_q 137 | marker.pose.orientation.y = y_q 138 | marker.pose.orientation.z = z_q 139 | marker.pose.orientation.w = w_q 140 | # Size: 141 | marker.scale.x = M_data.M[ii].m_i[max_p].a 142 | marker.scale.y = M_data.M[ii].m_i[max_p].b 143 | marker.scale.z = object_height 144 | # Location of the TV: 145 | marker.pose.position.x = M_data.M[ii].m_i[max_p].x_center 146 | marker.pose.position.y = M_data.M[ii].m_i[max_p].y_center 147 | marker.pose.position.z = marker.scale.z/2 148 | # Name: 149 | marker.ns = name + str(ii) 150 | list_marker.markers.append(marker) 151 | 152 | # Text adding: 153 | marker_name.header.frame_id = 'map' 154 | marker_name.header.stamp = rospy.Time.now() 155 | # Orientation of the text 156 | marker_name.pose.orientation.x = x_q 157 | marker_name.pose.orientation.y = y_q 158 | marker_name.pose.orientation.z = z_q 159 | marker_name.pose.orientation.w = w_q 160 | # Colour of text 161 | marker_name.color.r = 200 162 | marker_name.color.g = 200 163 | marker_name.color.b = 200 164 | marker_name.color.a = 1 165 | # Rest of things: 166 | marker_name.id = 10 167 | marker_name.type = 9 168 | marker_name.action = 0 169 | marker_name.lifetime.secs = 0 170 | marker_name.pose.position.x = M_data.M[ii].m_i[max_p].x_center 171 | marker_name.pose.position.y = M_data.M[ii].m_i[max_p].y_center 172 | marker_name.pose.position.z = 2*marker.pose.position.z + 0.1 173 | # Size of the text 174 | marker_name.scale.x = 0.2 175 | marker_name.scale.y = 0.2 176 | marker_name.scale.z = 0.2 177 | marker_name.text = name 178 | marker_name.ns = name + str(ii) 179 | 180 | list_marker.markers.append(marker_name) 181 | continue 182 | 183 | if M_data.M[ii].m_i[max_p].cls_num in A_Elliptical: 184 | # Test for putting an elliptical in the map. 185 | marker.header.frame_id = 'map' 186 | marker.header.stamp = rospy.Time.now() 187 | 188 | # Rest of things: 189 | marker.id = 1 190 | marker.type = 3 191 | marker.action = 0 192 | marker.lifetime.secs = 0 193 | # Orientation of the TV 194 | marker.pose.orientation.x = x_q 195 | marker.pose.orientation.y = y_q 196 | marker.pose.orientation.z = z_q 197 | marker.pose.orientation.w = w_q 198 | # Size: 199 | marker.scale.x = 2*M_data.M[ii].m_i[max_p].a 200 | marker.scale.y = 2*M_data.M[ii].m_i[max_p].b 201 | marker.scale.z = object_height 202 | # Location of the TV: 203 | marker.pose.position.x = M_data.M[ii].m_i[max_p].x_center 204 | marker.pose.position.y = M_data.M[ii].m_i[max_p].y_center 205 | marker.pose.position.z = marker.scale.z/2 206 | 207 | 208 | marker.color.r = 50 209 | marker.color.g = 50 210 | marker.color.b = 100 211 | marker.color.a = 1 212 | marker.ns = name + str(ii) 213 | marker_name.text = name 214 | marker_name.ns = name + str(ii) 215 | 216 | 217 | list_marker.markers.append(marker) 218 | 219 | # Text adding: 220 | marker_name.header.frame_id = 'map' 221 | marker_name.header.stamp = rospy.Time.now() 222 | # Orientation of the text 223 | marker_name.pose.orientation.x = x_q 224 | marker_name.pose.orientation.y = y_q 225 | marker_name.pose.orientation.z = z_q 226 | marker_name.pose.orientation.w = w_q 227 | # Colour of text 228 | marker_name.color.r = 200 229 | marker_name.color.g = 200 230 | marker_name.color.b = 200 231 | marker_name.color.a = 1 232 | # Rest of things: 233 | marker_name.id = 10 234 | marker_name.type = 9 235 | marker_name.action = 0 236 | marker_name.lifetime.secs = 0 237 | marker_name.pose.position.x = M_data.M[ii].m_i[max_p].x_center 238 | marker_name.pose.position.y = M_data.M[ii].m_i[max_p].y_center 239 | marker_name.pose.position.z = 2*marker.pose.position.z + 0.1 240 | # Size of the text 241 | marker_name.scale.x = 0.2 242 | marker_name.scale.y = 0.2 243 | marker_name.scale.z = 0.2 244 | 245 | list_marker.markers.append(marker_name) 246 | continue 247 | 248 | #publish: 249 | O_publisher.publish(list_marker) 250 | for jj in range(len(list_marker.markers)): 251 | list_marker.markers[jj].action = 2 252 | 253 | 254 | -------------------------------------------------------------------------------- /object_mapping/object_mapper/obj_class.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Libraries: 4 | import rospy 5 | import numpy as np 6 | 7 | 8 | # updating location and sizes: 9 | def Theta_updater(x_old,x_new, 10 | y_old,y_new, 11 | r_old,r_new, 12 | a_old,a_new, 13 | b_old,b_new, 14 | phi_old,phi_new): 15 | '''Update exsisting object values. 16 | Return updated Mi''' 17 | x = (x_old + x_new)/2 18 | y = (y_old + y_new)/2 19 | r = (r_old + r_new)/2 20 | a = (a_old + a_new)/2 21 | b = (b_old + b_new)/2 22 | phi = (phi_old + phi_new)/2 23 | 24 | return x , y , r , a , b , phi 25 | 26 | 27 | # Updating the Probabilities of every object with consideration for the old ones: 28 | def Updated_Probabilities_and_Cls(old_probability,new_probability,new_cls): 29 | '''Update the probability and the class of founded object. 30 | Returns [updated probability , the updated class number] ''' 31 | if len(np.array(old_probability)) < 1: 32 | new_cls = np.argmax(new_probability)+1 33 | updated_prob = new_probability 34 | else: 35 | updated_prob = 0.5 * (np.array(old_probability) + np.array(new_probability)) 36 | sumU = np.sum(updated_prob) 37 | updated_prob = updated_prob / sumU 38 | new_cls = np.argmax(updated_prob)+1 39 | return updated_prob, new_cls 40 | 41 | # Calculating the distance between two points: 42 | def _distance(pt_1, pt_2): 43 | '''Calculate distance betwenn two points. 44 | Returns distance value''' 45 | pt_1 = np.array((pt_1[0,0], pt_1[0,1])) 46 | pt_2 = np.array((pt_2[0], pt_2[1])) 47 | return np.linalg.norm(pt_1-pt_2) 48 | 49 | def closest_node(node, nodes): 50 | '''Finding the closest point to a given point. 51 | Return [distance , closest point index] ''' 52 | i = 0 53 | j = 0 54 | dist = 9999999 55 | for n in nodes: 56 | if _distance(node, n) <= dist: 57 | dist = _distance(node, n) 58 | j = i 59 | i += 1 60 | return [dist , j] 61 | 62 | def Search_Radius(r,a,b): 63 | '''Return the maximum value from a,b,r. 64 | Fo example, if object is circle, it will return the value of r''' 65 | return np.amax([r,a,b]) 66 | 67 | class Object_Map_cls(): 68 | 69 | 70 | def __init__(self,x_center=0,y_center=0,r=0,a=0,b=0,angle=0,cls_num=0,prob_distribution=[],viewed_number=0): 71 | 72 | self.x_center = x_center 73 | self.y_center = y_center 74 | self.r = r 75 | self.a = a 76 | self.b = b 77 | self.angle = angle 78 | self.cls_num = cls_num 79 | self.prob_distribution = prob_distribution 80 | 81 | -------------------------------------------------------------------------------- /object_mapping/object_mapper/obj_class.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/or-tal-robotics/object_map/b831d1e2237f162eca598f4a14fca2b4c655ce15/object_mapping/object_mapper/obj_class.pyc -------------------------------------------------------------------------------- /object_mapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | object_mapping 4 | 0.0.1 5 | The object_mapping package 6 | 7 | 8 | Amit Elbaz 9 | 10 | Amit Elbaz 11 | BSD 12 | 13 | message_generation 14 | 15 | 16 | 17 | 18 | 19 | message_runtime 20 | 21 | 22 | 23 | 24 | catkin 25 | rospy 26 | std_msgs 27 | rospy 28 | std_msgs 29 | rospy 30 | std_msgs 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | --------------------------------------------------------------------------------