├── README.md
├── arduino_encoder
    └── arduino_encoder.ino
└── encoder_odometry
    ├── CMakeLists.txt
    ├── README.md
    ├── launch
        ├── display.launch
        └── final.launch
    ├── package.xml
    ├── robot1.urdf
    └── src
        └── odometry.cpp


/README.md:
--------------------------------------------------------------------------------
1 | # encoder-odometry
2 | Pose calculation using wheel encoder data,
3 | 


--------------------------------------------------------------------------------
/arduino_encoder/arduino_encoder.ino:
--------------------------------------------------------------------------------
 1 | #include<ros.h>
 2 | #include<geometry_msgs/Vector3.h>
 3 | 
 4 | ros::NodeHandle nh;
 5 | geometry_msgs::Vector3 data;
 6 | ros::Publisher pos("pos",&data);
 7 | 
 8 | long left_count=0, right_count=0;
 9 | 
10 | void setup()
11 | {
12 |  Serial.begin(57600);
13 |  nh.initNode();
14 |  nh.advertise(pos);
15 |  attachInterrupt(0,isr_right,FALLING);
16 |  attachInterrupt(1,isr_left,FALLING);
17 |  pinMode(11,OUTPUT);
18 | }
19 | 
20 | 
21 | void loop()
22 | {
23 |  digitalWrite(11,HIGH);
24 |  data.x=left_count;
25 |  data.y=right_count;
26 |  Serial.print("right:   "); Serial.print(right_count);  Serial.print("left:   "); Serial.println(left_count);
27 |  pos.publish(&data);
28 |  nh.spinOnce();
29 |  delay(2);
30 | }
31 | 
32 | void isr_right()
33 | {
34 |   right_count++;
35 | }
36 | 
37 | void isr_left()
38 | {
39 |   left_count++;
40 | }
41 | 
42 | 
43 | 


--------------------------------------------------------------------------------
/encoder_odometry/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(encoder_odometry)
 3 | 
 4 | find_package(catkin REQUIRED COMPONENTS
 5 | 	tf
 6 | 	geometry_msgs
 7 | 	nav_msgs
 8 |         roscpp
 9 |         rospy
10 |         std_msgs
11 | )
12 | 
13 | 
14 | catkin_package()
15 | 
16 | ###########
17 | ## Build ##
18 | ###########
19 | 
20 | ## Specify additional locations of header files
21 | ## Your package locations should be listed before other locations
22 | # include_directories(include)
23 | include_directories(
24 |   ${catkin_INCLUDE_DIRS}
25 | )
26 | 
27 | ## Declare a C++ library
28 | # add_library(encoder_odometry
29 | #   src/${PROJECT_NAME}/encoder_odometry.cpp
30 | # )
31 | 
32 | ## Add cmake target dependencies of the library
33 | ## as an example, code may need to be generated before libraries
34 | ## either from message generation or dynamic reconfigure
35 | # add_dependencies(encoder_odometry ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
36 | 
37 | ## Declare a C++ executable
38 | add_executable(odometry src/odometry.cpp)
39 | target_link_libraries(odometry
40 |    ${catkin_LIBRARIES}
41 | )
42 | 
43 | 


--------------------------------------------------------------------------------
/encoder_odometry/README.md:
--------------------------------------------------------------------------------
1 | ROS node to calculate pose of the robot. 
2 | 
3 | Subscribes to the 'pos' topic published by the arduino.
4 | 


--------------------------------------------------------------------------------
/encoder_odometry/launch/display.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |  <arg name="model" />
 3 |  <arg name="gui" default="False" />
 4 |  <param name="robot_description" textfile="$(arg model)" />
 5 |  <param name="use_gui" value="$(arg gui)"/>
 6 |  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
 7 |  <node name="rviz" pkg="rviz" type="rviz" required="true" />
 8 |  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
 9 | </launch>
10 | 


--------------------------------------------------------------------------------
/encoder_odometry/launch/final.launch:
--------------------------------------------------------------------------------
  1 | <launch>
  2 |  <!-- "camera" should uniquely identify the device. All topics are pushed down
  3 |  into the "camera" namespace, and it is prepended to tf frame ids. -->
  4 |  <arg name="camera" default="camera" />
  5 |  <arg name="tf_prefix" default="" />
  6 |  <arg name="rgb_frame_id" default="$(arg tf_prefix)/$(arg camera)_rgb_optical_frame" />
  7 |  <arg name="depth_frame_id" default="$(arg tf_prefix)/$(arg camera)_depth_optical_frame" />
  8 |  <!-- device_id can have the following formats:
  9 |  "B00367707227042B": Use device with given serial number
 10 |  "#1" : Use first device found
 11 |  "2@3" : Use device on USB bus 2, address 3
 12 |  "2@0" : Use first device found on USB bus 2
 13 |  -->
 14 |  <arg name="device_id" default="#1" />
 15 |  <!-- By default, calibrations are stored to Error! Hyperlink reference not valid.,
 16 |  where ${NAME} is of the form "[rgb|depth]_[serial#]", e.g. "depth_B00367707227042B".
 17 |  See camera_info_manager docs for calibration URL details. -->
 18 |  <arg name="rgb_camera_info_url" default="" />
 19 |  <arg name="depth_camera_info_url" default="" />
 20 |  <!-- Use OpenNI's factory-calibrated depth->RGB registration? -->
 21 |  <arg name="depth_registration" default="false" />
 22 |  <!-- Arguments for remapping all device namespaces -->
 23 |  <arg name="rgb" default="rgb" />
 24 |  <arg name="ir" default="ir" />
 25 |  <arg name="depth" default="depth" />
 26 |  <arg name="depth_registered" default="depth_registered" />
 27 |  <arg name="projector" default="projector" />
 28 |  <!-- Optionally suppress loading the driver nodelet and/or publishing the default tf
 29 |  tree. Useful if you are playing back recorded raw data from a bag, or are
 30 |  supplying a more accurate tf tree from calibration. -->
 31 |  <arg name="load_driver" default="true" />
 32 |  <arg name="publish_tf" default="true" />
 33 |  <!-- Processing Modules -->
 34 |  <arg name="rgb_processing" default="true"/>
 35 |  <arg name="ir_processing" default="true"/>
 36 |  <arg name="depth_processing" default="true"/>
 37 |  <arg name="depth_registered_processing" default="true"/>
 38 |  <arg name="disparity_processing" default="true"/>
 39 |  <arg name="disparity_registered_processing" default="true"/>
 40 |  <arg name="hw_registered_processing" default="true" />
 41 |  <arg name="sw_registered_processing" default="true" />
 42 |  <!-- Disable bond topics by default -->
 43 |  <arg name="bond" default="false" /> <!-- DEPRECATED, use respawn arg instead -->
 44 |  <arg name="respawn" default="$(arg bond)" />
 45 |  <!-- Worker threads for the nodelet manager -->
 46 |  <arg name="num_worker_threads" default="4" />
 47 |  <!-- Push down all topics/nodelets into "camera" namespace -->
 48 |  <group ns="$(arg camera)">
 49 |  <!-- Start nodelet manager in top-level namespace -->
 50 |  <arg name="manager" value="$(arg camera)_nodelet_manager" />
 51 |  <arg name="debug" default="false" /> <!-- Run manager in GDB? -->
 52 |  <include file="$(find rgbd_launch)/launch/includes/manager.launch.xml">
 53 |  <arg name="name" value="$(arg manager)" />
 54 |  <arg name="debug" value="$(arg debug)" />
 55 |  <arg name="num_worker_threads" value="$(arg num_worker_threads)" />
 56 |  </include>
 57 |  <!-- Load driver -->
 58 |  <include if="$(arg load_driver)"
 59 |  file="$(find openni_launch)/launch/includes/device.launch.xml">
 60 |  <!-- Could really use some syntactic sugar for this -->
 61 |  <arg name="manager" value="$(arg manager)" />
 62 |  <arg name="device_id" value="$(arg device_id)" />
 63 |  <arg name="rgb_frame_id" value="$(arg rgb_frame_id)" />
 64 |  <arg name="depth_frame_id" value="$(arg depth_frame_id)" />
 65 |  <arg name="rgb_camera_info_url" value="$(arg rgb_camera_info_url)" />
 66 |  <arg name="depth_camera_info_url" value="$(arg depth_camera_info_url)" />
 67 |  <arg name="depth_registration" value="$(arg depth_registration)" />
 68 |  <arg name="rgb" value="$(arg rgb)" />
 69 |  <arg name="ir" value="$(arg ir)" />
 70 |  <arg name="depth" value="$(arg depth)" />
 71 |  <arg name="depth_registered" value="$(arg depth_registered)" />
 72 |  <arg name="projector" value="$(arg projector)" />
 73 |  <arg name="respawn" value="$(arg respawn)" />
 74 |  </include>
 75 |  <!-- Load standard constellation of processing nodelets -->
 76 |  <include file="$(find rgbd_launch)/launch/includes/processing.launch.xml">
 77 |  <arg name="manager" value="$(arg manager)" />
 78 |  <arg name="rgb" value="$(arg rgb)" />
 79 |  <arg name="ir" value="$(arg ir)" />
 80 |  <arg name="depth" value="$(arg depth)" />
 81 |  <arg name="depth_registered" value="$(arg depth_registered)" />
 82 |  <arg name="projector" value="$(arg projector)" />
 83 |  <arg name="respawn" value="$(arg respawn)" />
 84 |  <arg name="rgb_processing" value="$(arg rgb_processing)" />
 85 |  <arg name="ir_processing" value="$(arg ir_processing)" />
 86 |  <arg name="depth_processing" value="$(arg depth_processing)" />
 87 |  <arg name="depth_registered_processing" value="$(arg depth_registered_processing)" />
 88 |  <arg name="disparity_processing" value="$(arg disparity_processing)" />
 89 |  <arg name="disparity_registered_processing" value="$(arg disparity_registered_processing)" />
 90 |  <arg name="hw_registered_processing" value="$(arg hw_registered_processing)" />
 91 |  <arg name="sw_registered_processing" value="$(arg sw_registered_processing)" />
 92 |  </include>
 93 |  </group> <!-- camera -->
 94 |  <!-- Load reasonable defaults for the relative pose between cameras -->
 95 |  <include if="$(arg publish_tf)"
 96 |  file="$(find rgbd_launch)/launch/kinect_frames.launch">
 97 |  <arg name="camera" value="$(arg camera)" />
 98 |  <arg name="tf_prefix" value="$(arg tf_prefix)" />
 99 |  </include>
100 |  <arg name="model" default="urdf/robot1.urdf"/>
101 |  <arg name="gui" default="False" />
102 |  <param name="robot_description" textfile="$(arg model)" />
103 |  <param name="use_gui" value="$(arg gui)"/>
104 |  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
105 |  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
106 |  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find slam)/urdf/urdf.rviz" required="true"/>
107 |  <node name="odometry_publisher" pkg="slam" type="odometry_pub" /> 
108 |  <node pkg="rosserial_python" type="serial_node.py" name="Arduino" output="screen">
109 |  <param name="~port" value="/dev/ttyACM1" />
110 |  <param name="~baud" value="57600" />
111 |  </node>
112 |  <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
113 | <remap from="image" to="/camera/depth/image_raw"/>
114 |  </node>
115 | </launch>
116 | 


--------------------------------------------------------------------------------
/encoder_odometry/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>encoder_odometry</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The encoder_odometry 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="rohan@todo.todo">rohan</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 mutiple 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/encoder_odometry</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, mutiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintianers, 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 build_depend for packages you need at compile time: -->
35 |   <!--   <build_depend>message_generation</build_depend> -->
36 |   <!-- Use buildtool_depend for build tool packages: -->
37 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
38 |   <!-- Use run_depend for packages you need at runtime: -->
39 |   <!--   <run_depend>message_runtime</run_depend> -->
40 |   <!-- Use test_depend for packages you need only for testing: -->
41 |   <!--   <test_depend>gtest</test_depend> -->
42 |   <buildtool_depend>catkin</buildtool_depend>
43 |   <build_depend>roscpp</build_depend>
44 |   <build_depend>rospy</build_depend>
45 |   <build_depend>std_msgs</build_depend>
46 |   <run_depend>roscpp</run_depend>
47 |   <run_depend>rospy</run_depend>
48 |   <run_depend>std_msgs</run_depend>
49 | 
50 | 
51 |   <!-- The export tag contains other, unspecified, tags -->
52 |   <export>
53 |     <!-- Other tools can request additional information be placed here -->
54 | 
55 |   </export>
56 | </package>


--------------------------------------------------------------------------------
/encoder_odometry/robot1.urdf:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <robot name="hash_slam">
 3 |  <link name="base_link" >
 4 |  <visual>
 5 |  <geometry>
 6 |  <box size="0.47 0.28 0.07"/>
 7 |  </geometry>
 8 | <material name="black">
 9 |  <color rgba="1 0 1 0.5"/>
10 |  </material>
11 |  </visual>
12 | </link>
13 | <link name="camera_link" >
14 | <visual>
15 | <origin xyz="0 0 -0.015" rpy="0 0 0" /> 
16 |  <geometry>
17 |  <cylinder length="0.08" radius="0.02"/>
18 |  </geometry>
19 | <material name="blue">
20 |  <color rgba="0 0 1 0.5"/>
21 | </material>
22 |  </visual>
23 | </link>
24 | <link name="LW" >
25 | <visual>
26 | <origin xyz="0 0 0" rpy="1.57 0 0" />
27 |  <geometry>
28 |  <cylinder length="0.045" radius="0.05"/>
29 |  </geometry>
30 |  </visual>
31 | </link>
32 |  <link name="RW" >
33 | <visual>
34 | <origin xyz="0 0 0" rpy="-1.57 0 0" />
35 |  <geometry>
36 |  <cylinder length="0.045" radius="0.05"/>
37 |  </geometry>
38 |  </visual>
39 | </link>
40 | <link name="rodr">
41 |  <visual>
42 |  <origin xyz="0 -0.02 0" rpy="1.57 0 0"/>
43 |  <geometry>
44 |  <cylinder length="0.04" radius="0.005"/>
45 |  </geometry>
46 |  <material name="white">
47 |  <color rgba="1 1 1 1"/>
48 |  </material>
49 |  </visual>
50 | </link>
51 | <link name="rodl">
52 |  <visual>
53 |  <origin xyz="0 0.02 0" rpy="1.57 0 0"/>
54 |  <geometry>
55 |  <cylinder length="0.04" radius="0.005"/>
56 |  </geometry>
57 |  <material name="white">
58 |  <color rgba="1 1 1 1"/>
59 |  </material>
60 |  </visual>
61 | </link>
62 | <joint name="base_link_camera_link" type="fixed">
63 | <origin xyz="0.185 0 0.07" rpy="0 0 0"/>
64 | <parent link="base_link"/>
65 |  <child link="camera_link"/>
66 | </joint>
67 | <joint name="base_link_rodr" type="fixed">
68 | <origin xyz="0 -0.14 -0.045" rpy="0 0 0"/>
69 | <parent link="base_link"/>
70 |  <child link="rodr"/>
71 | </joint>
72 | <joint name="base_link_rodl" type="fixed">
73 | <origin xyz="0 0.14 -0.045" rpy="0 0 0"/>
74 | <parent link="base_link"/>
75 |  <child link="rodl"/>
76 | </joint>
77 | <joint name="rodr_RW" type="continuous">
78 | <origin xyz="0 -0.04 0" rpy="0 0 0"/>
79 | <axis xyz="0 1 0" /> 
80 | <parent link="rodr"/>
81 |  <child link="RW"/>
82 | </joint>
83 | <joint name="rodl_LW" type="continuous">
84 | <origin xyz="0 0.04 0" rpy="0 0 0"/>
85 | <axis xyz="0 1 0" /> 
86 | <parent link="rodl"/>
87 |  <child link="LW"/>
88 | </joint>
89 | </robot>
90 | 


--------------------------------------------------------------------------------
/encoder_odometry/src/odometry.cpp:
--------------------------------------------------------------------------------
  1 | #include "ros/ros.h"
  2 | #include <geometry_msgs/Vector3.h>
  3 | #include <tf/transform_broadcaster.h>
  4 | #include <nav_msgs/Odometry.h>
  5 | 
  6 | long _PreviousLeftEncoderCounts = 0;
  7 | long _PreviousRightEncoderCounts = 0;
  8 | ros::Time current_time, last_time;
  9 | double DistancePerCount = (3.14159265 * 0.1)/30; //the wheel diameter is 0.1m
 10 | //final odometric datas
 11 | double x;
 12 | double y;
 13 | double th;
 14 | double v_left;//left motor speed
 15 | double v_right;//right motor speed
 16 | double vth;//angular velocity of robot
 17 | double deltaLeft;//no of ticks in left encoder since last update
 18 | double deltaRight;//no of ticks in right encoder since last update
 19 | double dt;
 20 | double delta_distance;//distance moved by robot since last update
 21 | double delta_th;//corresponging change in heading
 22 | double delta_x ;//corresponding change in x direction
 23 | double delta_y;//corresponding change in y direction
 24 | #define PI 3.14159265
 25 | #define TwoPI 6.28318531
 26 | 
 27 | 
 28 | void WheelCallback(const geometry_msgs::Vector3::ConstPtr& ticks)
 29 | {
 30 |  current_time = ros::Time::now();
 31 |  deltaLeft = ticks->x - _PreviousLeftEncoderCounts;
 32 |  deltaRight = ticks->y - _PreviousRightEncoderCounts;
 33 |  dt = (current_time - last_time).toSec();
 34 |  v_left = deltaLeft * DistancePerCount/dt;
 35 |  v_right = deltaRight * DistancePerCount/dt;
 36 |  delta_distance=0.5f*(double)(deltaLeft+deltaRight)*DistancePerCount;
 37 |  delta_th = (double)(deltaRight-deltaLeft)*DistancePerCount/0.36f; //Distance between the two wheels is 0.36m
 38 |  delta_x = delta_distance*(double)cos(th);
 39 |  delta_y = delta_distance*(double)sin(th);
 40 |  x += delta_x;
 41 |  y += delta_y;
 42 |  th += delta_th;
 43 |  if (th > PI)
 44 |  th -= TwoPI;
 45 |  else
 46 |  if ( th <= -PI)
 47 |  th += TwoPI;
 48 |  _PreviousLeftEncoderCounts = ticks->x;
 49 |  _PreviousRightEncoderCounts = ticks->y;
 50 |  last_time = current_time;
 51 | }
 52 | 
 53 | 
 54 | int main(int argc, char **argv)
 55 | {
 56 |  ros::init(argc, argv, "odometry_publisher");
 57 |  ros::NodeHandle n;
 58 |  ros::Subscriber sub = n.subscribe("pos", 100, WheelCallback);
 59 |  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
 60 |  tf::TransformBroadcaster odom_broadcaster;
 61 |  ros::Rate r(40);
 62 | 
 63 |  while(n.ok()){
 64 |     //since all odometry is 6DOF we'll need a quaternion created from yaw
 65 |     geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
 66 |     //first, we'll publish the transform over tf
 67 |     geometry_msgs::TransformStamped odom_trans;
 68 |     odom_trans.header.stamp = current_time;
 69 |     odom_trans.header.frame_id = "odom";
 70 |     odom_trans.child_frame_id = "base_link";
 71 |     odom_trans.transform.translation.x = x;
 72 |     odom_trans.transform.translation.y = y;
 73 |     odom_trans.transform.translation.z = 0.0;
 74 |     odom_trans.transform.rotation = odom_quat;
 75 |     //send the transform
 76 |     odom_broadcaster.sendTransform(odom_trans);
 77 |     //next, we'll publish the odometry message over ROS
 78 |     nav_msgs::Odometry odom;
 79 |     odom.header.stamp = current_time;
 80 |     odom.header.frame_id = "odom";
 81 |     //set the position
 82 |     odom.pose.pose.position.x = x;
 83 |     odom.pose.pose.position.y = y;
 84 |     odom.pose.pose.position.z = 0.0;
 85 |     odom.pose.pose.orientation = odom_quat;
 86 |     //set the velocity
 87 |     odom.child_frame_id = "base_link";
 88 |     odom.twist.twist.linear.x=delta_x/dt;
 89 |     odom.twist.twist.linear.y=delta_y/dt;
 90 |     odom.twist.twist.angular.z = delta_th/dt; 
 91 |     
 92 |     ROS_INFO("Position x = %d and Position y = %d ",x,y);
 93 | 	
 94 |     //publish the message
 95 |     odom_pub.publish(odom);
 96 |     last_time = current_time;
 97 |     ros::spinOnce();
 98 |     r.sleep();
 99 |     }
100 | }
101 | 


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