Encoder Odometry

CMakeLists.txt

@@ -20,7 +20,7 @@ find_package(catkin REQUIRED COMPONENTS
   mavros_msgs
   nav_msgs
   tf
-  tf2
+  tf2_ros
   tf2_geometry_msgs
   cv_bridge
 	state_controller
@@ -63,6 +63,7 @@ add_message_files(
   Hemisphere.msg
   RoboClawStats.msg
   ImuSafety.msg
+  DiffDrive.msg
 )
 
 # Generate services in the 'srv' folder
@@ -134,6 +135,7 @@ add_executable( idk src/arm_drone.cpp )
 add_executable( road_detection src/road_detection.cpp )
 add_executable(imu_safety src/imu_safety/ImuSafety.cpp)
 add_executable(imu_base src/ImuBase/ImuBase.cpp)
+add_executable(enc_odom src/EncOdom/EncOdom.cpp)
 add_executable(Watchdog src/Watchdog/Watchdog.cpp)
 
 # Specify libraries to link a library or executable target against
@@ -144,12 +146,14 @@ target_link_libraries( idk ${catkin_LIBRARIES} )
 target_link_libraries( road_detection ${OpenCV_LIBS} )
 target_link_libraries(imu_safety ${catkin_LIBRARIES})
 target_link_libraries(imu_base ${catkin_LIBRARIES})
+target_link_libraries(enc_odom ${catkin_LIBRARIES})
 target_link_libraries(Watchdog ${catkin_LIBRARIES})
 
 # Add cmake target dependencies of the executable
 # same as for the library above
 add_dependencies(Hemisphere ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(imu_safety ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(enc_odom ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 
 #IF(Qt5Widgets_FOUND)

launch/localization.launch

@@ -10,4 +10,5 @@
   <!-- <node name="hemisphere_tf" pkg="tf" type="static_transform_publisher" args="1.94 0 1.49 0 0 0 50" /> -->
   <node name="laser2_to_base" pkg="tf" type="static_transform_publisher" args="2.20 0 1.30 0 1.571 0 /base_link /laser 50" />
   <node name="imu_to_base" pkg="tf" type="static_transform_publisher" args = "0 0 0 0 1.571 1.571 /base_link /base_imu 100"/>
+  <node name="enc_to_base" pkg="tf2_ros" type="static_transform_publisher" args = "0 0 0 0 0 0 /base_link /enc_odom"/>
 </launch>

msg/DiffDrive.msg

@@ -0,0 +1,4 @@
+# This represents the distance between the two wheels
+float64 wheelbase
+float64 rvel
+float64 lvel

src/EncOdom/EncOdom.cpp

@@ -0,0 +1,38 @@
+#include "EncOdom.h"
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+EncOdom::EncOdom()
+  : sub(nh.subscribe("diffdrive_raw", 100, &EncOdom::callback, this))
+  , then(ros::Time::now())
+{
+  transformStamped.header.frame_id = "odom";
+  transformStamped.child_frame_id = "enc_link";
+}
+
+void EncOdom::callback(const gravl::DiffDrive::ConstPtr& msg)
+{
+  const auto now = ros::Time::now();
+  const auto angularVel = (msg->rvel - msg->lvel) / msg->wheelbase;
+  const auto tangentialVel = (msg->rvel + msg->lvel) / 2;
+  const auto dt = (now - then).toSec();
+
+  tf2::Quaternion oldOrientation;
+  tf2::convert(transformStamped.transform.rotation, oldOrientation);
+  const auto newOrientation = oldOrientation * tf2::Vector3(0, 0, angularVel * dt);
+  tf2::convert(newOrientation, transformStamped.transform.rotation);
+
+  tf2::Vector3 oldTranslation;
+  tf2::convert(transformStamped.transform.translation, oldTranslation);
+  const auto deltaPosition = tf2::quatRotate(newOrientation, tf2::Vector3(0, 0, tangentialVel * dt));
+  tf2::convert(oldTranslation + deltaPosition, transformStamped.transform.translation);
+
+  br.sendTransform(transformStamped);
+  then = now;
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "enc_odom");
+  EncOdom encOdom;
+  ros::spin();
+}

src/EncOdom/EncOdom.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include <gravl/DiffDrive.h>
+#include <tf2_ros/transform_broadcaster.h>
+
+class EncOdom
+{
+public:
+  EncOdom();
+
+private:
+  void callback(const gravl::DiffDrive::ConstPtr& msg);
+
+  ros::NodeHandle nh;
+  tf2_ros::TransformBroadcaster br;
+  ros::Subscriber sub;
+  geometry_msgs::TransformStamped transformStamped;
+  ros::Time then;
+};