├── README.md
├── launch
    └── ekf_template.launch
├── src
    ├── odomsubscriber.py
    ├── command.py
    └── imu_subscriber.py
├── package.xml
├── params
    └── ekf_template.yaml
└── CMakeLists.txt


/README.md:
--------------------------------------------------------------------------------
1 | # Localization


--------------------------------------------------------------------------------
/launch/ekf_template.launch:
--------------------------------------------------------------------------------
1 | 
2 | <launch>
3 |   <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true">
4 |     <rosparam command="load" file="localization/params/ekf_template.yaml" />
5 |  </node>
6 | </launch>
7 | 
8 | 


--------------------------------------------------------------------------------
/src/odomsubscriber.py:
--------------------------------------------------------------------------------
 1 | #!usr/bin/env python
 2 | import rospy
 3 | from geometry_msgs.msg import Twist
 4 | 
 5 | def callback(msg):
 6 | 	linear_vel_x=msg.linear.x
 7 | 	linear_vel_y=msg.linear.y
 8 | 	linear_vel_z=msg.linear.z
 9 | 	linear_data=[linear_vel_x,linear_vel_y,linear_vel_z]
10 | 	angular_vel_x=msg.angular.x
11 | 	angular_vel_y=msg.angular.y
12 | 	angular_vel_z=msg.angular.z
13 | 	angular_data=[angular_vel_x,angular_vel_y,angular_vel_z]
14 | 	print(linear_data,angular_data)
15 | def main():
16 | 	rospy.init_node('odom')
17 | 	rospy.Subsrciber('mobile_base/commands/velocity',Twist,callback)
18 | 	rospy.spin()
19 | if __name__='__main()__':
20 | 	main()
21 | 


--------------------------------------------------------------------------------
/src/command.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | import rospy
 4 | import tf
 5 | from geometry_msgs.msg import Twist,PoseStamped,Quaternion
 6 | from geometry_msgs.msg import PoseWithCovarianceStamped
 7 | from nav_msgs.msg import Odometry
 8 | from std_msgs.msg import Empty,Char,UInt8
 9 | from sensor_msgs.msg import Imu
10 | 
11 | def command_inputs():
12 | 
13 |     rospy.init_node('Command')
14 |     pub = rospy.Publisher('command',UInt8,queue_size = 10)
15 |     while(1):
16 |         command = input('Command Input: ')
17 |         print command
18 |         pub.publish(command)
19 |         
20 | 
21 | 
22 | if __name__ == '__main__':
23 | 	command_inputs()
24 | 
25 | 


--------------------------------------------------------------------------------
/src/imu_subscriber.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | import rospy
 4 | import tf
 5 | from geometry_msgs.msg import Twist,PoseStamped,Quaternion
 6 | from geometry_msgs.msg import PoseWithCovarianceStamped
 7 | from nav_msgs.msg import Odometry
 8 | from std_msgs.msg import Empty
 9 | from sensor_msgs.msg import Imu
10 | 
11 | 
12 | def callbackimu(data,pub):
13 |     pose = PoseStamped()
14 |     pose.header = data.header
15 |     quaternion = (
16 |     data.orientation.x,
17 |     data.orientation.y,
18 |     data.orientation.z,
19 |     data.orientation.w)
20 | 
21 |     euler = tf.transformations.euler_from_quaternion(quaternion)
22 | 
23 |     pose.pose.orientation.x = euler[0]
24 |     pose.pose.orientation.y = euler[1]
25 |     pose.pose.orientation.z = euler[2]
26 |     pub.publish(pose)
27 | 
28 | def pubeuler():
29 |   
30 |     rospy.init_node('pub_euler')
31 |     pubimu = rospy.Publisher('euler_imu', PoseStamped,queue_size = 10)
32 | 
33 |     rospy.Subscriber('/android/imu',Imu,callbackimu, callback_args=pubimu)
34 | 
35 | 
36 |     rospy.spin()
37 | 
38 | 
39 | if __name__ == '__main__':
40 |     
41 |     
42 |     pubeuler()
43 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>localization</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The localization package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag -->
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="kushagra@todo.todo">kushagra</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>TODO</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but multiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/localization</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, multiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintainers, but could be -->
27 |   <!-- Example: -->
28 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29 | 
30 | 
31 |   <!-- The *depend tags are used to specify dependencies -->
32 |   <!-- Dependencies can be catkin packages or system dependencies -->
33 |   <!-- Examples: -->
34 |   <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
35 |   <!--   <depend>roscpp</depend> -->
36 |   <!--   Note that this is equivalent to the following: -->
37 |   <!--   <build_depend>roscpp</build_depend> -->
38 |   <!--   <exec_depend>roscpp</exec_depend> -->
39 |   <!-- Use build_depend for packages you need at compile time: -->
40 |   <!--   <build_depend>message_generation</build_depend> -->
41 |   <!-- Use build_export_depend for packages you need in order to build against this package: -->
42 |   <!--   <build_export_depend>message_generation</build_export_depend> -->
43 |   <!-- Use buildtool_depend for build tool packages: -->
44 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
45 |   <!-- Use exec_depend for packages you need at runtime: -->
46 |   <!--   <exec_depend>message_runtime</exec_depend> -->
47 |   <!-- Use test_depend for packages you need only for testing: -->
48 |   <!--   <test_depend>gtest</test_depend> -->
49 |   <!-- Use doc_depend for packages you need only for building documentation: -->
50 |   <!--   <doc_depend>doxygen</doc_depend> -->
51 |   <buildtool_depend>catkin</buildtool_depend>
52 |   <build_depend>rospy</build_depend>
53 |   <build_depend>std_msgs</build_depend>
54 |   <build_export_depend>rospy</build_export_depend>
55 |   <build_export_depend>std_msgs</build_export_depend>
56 |   <exec_depend>rospy</exec_depend>
57 |   <exec_depend>std_msgs</exec_depend>
58 | 
59 | 
60 |   <!-- The export tag contains other, unspecified, tags -->
61 |   <export>
62 |     <!-- Other tools can request additional information be placed here -->
63 | 
64 |   </export>
65 | </package>
66 | 


--------------------------------------------------------------------------------
/params/ekf_template.yaml:
--------------------------------------------------------------------------------
 1 | #Configuraton for robot odometry EKF#
 2 | frequency: 10
 3 | 
 4 | two_d_mode : true
 5 | 
 6 | publish_tf: false
 7 | 
 8 | odom_frame: odom
 9 | 
10 | world_frame: map
11 | 
12 | #map_frame: map
13 | 
14 | odom0: /noisy_odom
15 | odom0_config:  [false, false, false,
16 | 		false, false, true,
17 | 		true, true, false,
18 | 		false, false, false,
19 | 		false, false, true,
20 | 		false, false, false]
21 | odom0_differential: false
22 | 
23 | imu0: /imu/data
24 | imu0_config:   [false, false, false
25 | 		false, false, true,
26 | 		false, false, false,
27 | 		false, false, true,
28 | 		true, false, false]
29 | imu0_differential: false	
30 | process_noise_covariance: [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
31 |                            0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
32 |                            0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
33 |                            0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
34 |                            0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
35 |                            0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
36 |                            0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
37 |                            0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
38 |                            0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
39 |                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
40 |                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
41 |                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
42 |                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
43 |                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
44 |                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]
45 | 
46 | initial_estimate_covariance: [1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
47 |                               0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
48 |                               0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
49 |                               0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
50 |                               0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
51 |                               0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
52 |                               0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
53 |                               0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
54 |                               0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
55 |                               0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
56 |                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
57 |                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
58 |                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
59 |                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
60 |                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]
61 | 
62 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | cmake_minimum_required(VERSION 2.8.3)
  2 | project(localization)
  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 | )
 14 | 
 15 | ## System dependencies are found with CMake's conventions
 16 | # find_package(Boost REQUIRED COMPONENTS system)
 17 | 
 18 | 
 19 | ## Uncomment this if the package has a setup.py. This macro ensures
 20 | ## modules and global scripts declared therein get installed
 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
 22 | # catkin_python_setup()
 23 | 
 24 | ################################################
 25 | ## Declare ROS messages, services and actions ##
 26 | ################################################
 27 | 
 28 | ## To declare and build messages, services or actions from within this
 29 | ## package, follow these steps:
 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in
 31 | ##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
 32 | ## * In the file package.xml:
 33 | ##   * add a build_depend tag for "message_generation"
 34 | ##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
 35 | ##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
 36 | ##     but can be declared for certainty nonetheless:
 37 | ##     * add a exec_depend tag for "message_runtime"
 38 | ## * In this file (CMakeLists.txt):
 39 | ##   * add "message_generation" and every package in MSG_DEP_SET to
 40 | ##     find_package(catkin REQUIRED COMPONENTS ...)
 41 | ##   * add "message_runtime" and every package in MSG_DEP_SET to
 42 | ##     catkin_package(CATKIN_DEPENDS ...)
 43 | ##   * uncomment the add_*_files sections below as needed
 44 | ##     and list every .msg/.srv/.action file to be processed
 45 | ##   * uncomment the generate_messages entry below
 46 | ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 47 | 
 48 | ## Generate messages in the 'msg' folder
 49 | # add_message_files(
 50 | #   FILES
 51 | #   Message1.msg
 52 | #   Message2.msg
 53 | # )
 54 | 
 55 | ## Generate services in the 'srv' folder
 56 | # add_service_files(
 57 | #   FILES
 58 | #   Service1.srv
 59 | #   Service2.srv
 60 | # )
 61 | 
 62 | ## Generate actions in the 'action' folder
 63 | # add_action_files(
 64 | #   FILES
 65 | #   Action1.action
 66 | #   Action2.action
 67 | # )
 68 | 
 69 | ## Generate added messages and services with any dependencies listed here
 70 | # generate_messages(
 71 | #   DEPENDENCIES
 72 | #   std_msgs
 73 | # )
 74 | 
 75 | ################################################
 76 | ## Declare ROS dynamic reconfigure parameters ##
 77 | ################################################
 78 | 
 79 | ## To declare and build dynamic reconfigure parameters within this
 80 | ## package, follow these steps:
 81 | ## * In the file package.xml:
 82 | ##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
 83 | ## * In this file (CMakeLists.txt):
 84 | ##   * add "dynamic_reconfigure" to
 85 | ##     find_package(catkin REQUIRED COMPONENTS ...)
 86 | ##   * uncomment the "generate_dynamic_reconfigure_options" section below
 87 | ##     and list every .cfg file to be processed
 88 | 
 89 | ## Generate dynamic reconfigure parameters in the 'cfg' folder
 90 | # generate_dynamic_reconfigure_options(
 91 | #   cfg/DynReconf1.cfg
 92 | #   cfg/DynReconf2.cfg
 93 | # )
 94 | 
 95 | ###################################
 96 | ## catkin specific configuration ##
 97 | ###################################
 98 | ## The catkin_package macro generates cmake config files for your package
 99 | ## Declare things to be passed to dependent projects
100 | ## INCLUDE_DIRS: uncomment this if your package contains header files
101 | ## LIBRARIES: libraries you create in this project that dependent projects also need
102 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need
103 | ## DEPENDS: system dependencies of this project that dependent projects also need
104 | catkin_package(
105 | #  INCLUDE_DIRS include
106 | #  LIBRARIES localization
107 | #  CATKIN_DEPENDS rospy std_msgs
108 | #  DEPENDS system_lib
109 | )
110 | 
111 | ###########
112 | ## Build ##
113 | ###########
114 | 
115 | ## Specify additional locations of header files
116 | ## Your package locations should be listed before other locations
117 | include_directories(
118 | # include
119 |   ${catkin_INCLUDE_DIRS}
120 | )
121 | 
122 | ## Declare a C++ library
123 | # add_library(${PROJECT_NAME}
124 | #   src/${PROJECT_NAME}/localization.cpp
125 | # )
126 | 
127 | ## Add cmake target dependencies of the library
128 | ## as an example, code may need to be generated before libraries
129 | ## either from message generation or dynamic reconfigure
130 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
131 | 
132 | ## Declare a C++ executable
133 | ## With catkin_make all packages are built within a single CMake context
134 | ## The recommended prefix ensures that target names across packages don't collide
135 | # add_executable(${PROJECT_NAME}_node src/localization_node.cpp)
136 | 
137 | ## Rename C++ executable without prefix
138 | ## The above recommended prefix causes long target names, the following renames the
139 | ## target back to the shorter version for ease of user use
140 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
141 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
142 | 
143 | ## Add cmake target dependencies of the executable
144 | ## same as for the library above
145 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
146 | 
147 | ## Specify libraries to link a library or executable target against
148 | # target_link_libraries(${PROJECT_NAME}_node
149 | #   ${catkin_LIBRARIES}
150 | # )
151 | 
152 | #############
153 | ## Install ##
154 | #############
155 | 
156 | # all install targets should use catkin DESTINATION variables
157 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
158 | 
159 | ## Mark executable scripts (Python etc.) for installation
160 | ## in contrast to setup.py, you can choose the destination
161 | # install(PROGRAMS
162 | #   scripts/my_python_script
163 | #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
164 | # )
165 | 
166 | ## Mark executables and/or libraries for installation
167 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
168 | #   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
169 | #   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
170 | #   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
171 | # )
172 | 
173 | ## Mark cpp header files for installation
174 | # install(DIRECTORY include/${PROJECT_NAME}/
175 | #   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
176 | #   FILES_MATCHING PATTERN "*.h"
177 | #   PATTERN ".svn" EXCLUDE
178 | # )
179 | 
180 | ## Mark other files for installation (e.g. launch and bag files, etc.)
181 | # install(FILES
182 | #   # myfile1
183 | #   # myfile2
184 | #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
185 | # )
186 | 
187 | #############
188 | ## Testing ##
189 | #############
190 | 
191 | ## Add gtest based cpp test target and link libraries
192 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_localization.cpp)
193 | # if(TARGET ${PROJECT_NAME}-test)
194 | #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
195 | # endif()
196 | 
197 | ## Add folders to be run by python nosetests
198 | # catkin_add_nosetests(test)
199 | 


--------------------------------------------------------------------------------