├── 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 | [](https://youtu.be/-H25q_Vcol8)
9 | ## Example - Experiment
10 | [](https://youtu.be/mQHh478gTg8)
11 | ## Pipelines
12 |
13 |
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 |
--------------------------------------------------------------------------------