add odometry calc

ranger_base/CMakeLists.txt

@@ -26,7 +26,8 @@ find_package(catkin REQUIRED COMPONENTS
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-
+find_package(Eigen3 REQUIRED)
+include_directories(${EIGEN3_INCLUDE_DIR})
 
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
@@ -129,9 +130,10 @@ catkin_package(
 include_directories(
   include
   ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
 )
 
-add_library(ranger_messenger STATIC src/ranger_messenger.cpp)
+add_library(ranger_messenger STATIC src/ranger_messenger.cpp src/ranger_model.cpp)
 target_link_libraries(ranger_messenger ${catkin_LIBRARIES})
 add_dependencies(ranger_messenger ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 

ranger_base/include/ranger_base/ranger_messenger.hpp

@@ -19,9 +19,13 @@
 #include <string>
 #include "ascent/Ascent.h"
 #include "ascent/Utility.h"
+#include "ranger_base/ranger_model.hpp"
 #include "ranger_base/ranger_params.hpp"
 #include "ugv_sdk/ranger_base.hpp"
 
+//#include <Eigen/Core>
+#include <eigen3/Eigen/Core>
+
 using namespace ros;
 using namespace ros::master;
 
@@ -53,6 +57,7 @@ class RangerROSMessenger {
   std::string odom_frame_;
   std::string base_frame_;
   std::string odom_topic_name_;
+  bool pub_odom_tf_;
 
   bool simulated_robot_ = false;
   int sim_control_rate_ = 50;
@@ -85,12 +90,19 @@ class RangerROSMessenger {
   static constexpr double steer_angle_tolerance = 0.005;  // ~+-0.287 degrees
 
   // speed variables
-  double linear_speed_ = 0.0;
-  double angular_angle_ = 0.0;
+  double linear_speed_ = 0.0;  // inear velocity
+  double angular_vel_ = 0.0;   // angule velocity
+  double x_linear_vel_ = 0.0;  // x direction linear velocity
+  double y_linear_vel_ = 0.0;  // y direction linear velocity
   double position_x_ = 0.0;
   double position_y_ = 0.0;
   double theta_ = 0.0;
 
+  // the transform of current position based on the origin
+  Eigen::Matrix4d curr_transform_{};
+
+  SystemPropagator<BicycleKinematics> model_;
+
   ros::Time last_time_;
   ros::Time current_time_;
 
@@ -99,7 +111,9 @@ class RangerROSMessenger {
   void RangerSettingCbk(const ranger_msgs::RangerSetting::ConstPtr &msg);
   double ConvertInnerAngleToCentral(double angle);
   double ConvertCentralAngleToInner(double angle);
-  void PublishOdometryToROS(double linear, double angular, double dt);
+  void PublishOdometryToROS(double linear, double angle_vel,
+                            double x_linear_vel, double y_linear_vel,
+                            double dt);
 };
 }  // namespace westonrobot
 

ranger_base/include/ranger_base/ranger_model.hpp

@@ -32,6 +32,9 @@ class BicycleKinematics
       double v{0.0};
       double delta{0.0};
     }
+
+    double v;
+    double delta;
   };
   using control_t = CtrlInput;
 
@@ -42,6 +45,7 @@ class BicycleKinematics
 
  private:
   control_t u_{0.0, 0.0};
+
   static constexpr double L = RangerParams::wheelbase;
 };
 }  // namespace westonrobot

ranger_base/launch/ranger_mini_base.launch

@@ -3,6 +3,7 @@
     <arg name="is_ranger_mini" default="true"/>
     <arg name="simulated_robot" default="false"/>
     <arg name="odom_topic_name" default="odom"/>
+    <arg name="pub_odom_tf" default="false" />
 
     <node name="ranger_base_node" pkg="ranger_base" type="ranger_base_node" output="screen">
         <param name="is_ranger_mini" type="bool" value="$(arg is_ranger_mini)" />
@@ -11,12 +12,6 @@
         <param name="odom_frame" type="string" value="odom" />
         <param name="base_frame" type="string" value="base_link" />
         <param name="odom_topic_name" type="string" value="$(arg odom_topic_name)" />  
-
-        <!-- <param name="is_ranger_mini" type="bool" value="true" />
-        <param name="port_name" type="string" value="can0" />
-        <param name="simulated_robot" type="bool" value="false" />
-        <param name="odom_frame" type="string" value="odom" />
-        <param name="base_frame" type="string" value="base_link" />
-        <param name="odom_topic_name" type="string" value="odom" /> -->
+        <param name="pub_odom_tf" type="bool" value="$(arg pub_odom_tf)" />
     </node>
 </launch>

ranger_base/src/ranger_base_node.cpp

@@ -42,6 +42,7 @@ int main(int argc, char **argv) {
   private_node.param<int>("control_rate", messenger.sim_control_rate_, 50);
   private_node.param<std::string>("odom_topic_name", messenger.odom_topic_name_,
                                   std::string("odom"));
+  private_node.param<bool>("pub_odom_tf", messenger.pub_odom_tf_, false);
 
   if (!messenger.simulated_robot_) {
     // connect to robot and setup ROS subscription

ranger_base/src/ranger_messenger.cpp

@@ -33,6 +33,8 @@ void RangerROSMessenger::SetupSubscription() {
       "/cmd_vel", 5, &RangerROSMessenger::TwistCmdCallback, this);
   ranger_setting_sub_ = nh_->subscribe<ranger_msgs::RangerSetting>(
       "/ranger_setting", 1, &RangerROSMessenger::RangerSettingCbk, this);
+
+  curr_transform_ = Eigen::Matrix4d::Identity();
 }
 
 void RangerROSMessenger::PublishStateToROS() {
@@ -127,8 +129,7 @@ void RangerROSMessenger::PublishStateToROS() {
 
   status_publisher_.publish(status_msg);
 
-  PublishOdometryToROS(status_msg.lateral_velocity, status_msg.steering_angle,
-                       dt);
+  PublishOdometryToROS(l_v, a_v, x_v, y_v, dt);
 
   last_time_ = current_time_;
 }
@@ -245,10 +246,66 @@ double RangerROSMessenger::ConvertCentralAngleToInner(double angle) {
   }
   return phi_i;
 }
-void RangerROSMessenger::PublishOdometryToROS(double linear, double angular,
-                                              double dt) {
+void RangerROSMessenger::PublishOdometryToROS(double linear, double angle_vel,
+                                              double x_linear_vel,
+                                              double y_linear_vel, double dt) {
   linear_speed_ = linear;
-  angular_angle_ = angular;
+  angular_vel_ = angle_vel;
+  x_linear_vel_ = x_linear_vel;
+  y_linear_vel_ = y_linear_vel;
+  double theta = angular_vel_ * dt;
+
+  // update
+  position_x_ +=
+      cos(theta_) * x_linear_vel_ * dt - sin(theta_) * y_linear_vel_ * dt;
+  position_y_ +=
+      sin(theta_) * x_linear_vel_ * dt + cos(theta_) * y_linear_vel_ * dt;
+  theta_ = theta_ + angular_vel_ * dt;
+
+  if (theta_ > M_PI) {
+    theta_ -= 2 * M_PI;
+  } else if (theta < -M_PI) {
+    theta_ += 2 * M_PI;
+  }
+  //  printf("theta_ %f,  theta cos angle: %f\n", curr_transform_(0, 0));
+
+  geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_);
+
+  if (pub_odom_tf_) {
+    // publish tf transformation
+    geometry_msgs::TransformStamped tf_msg;
+    tf_msg.header.stamp = current_time_;
+    tf_msg.header.frame_id = odom_frame_;
+    tf_msg.child_frame_id = base_frame_;
+
+    tf_msg.transform.translation.x = position_x_;
+    tf_msg.transform.translation.y = position_y_;
+    tf_msg.transform.translation.z = 0.0;
+    tf_msg.transform.rotation = odom_quat;
+
+    tf_broadcaster_.sendTransform(tf_msg);
+  }
+  // publish odometry and tf messages
+  nav_msgs::Odometry odom_msg;
+  odom_msg.header.stamp = current_time_;
+  odom_msg.header.frame_id = odom_frame_;
+  odom_msg.child_frame_id = base_frame_;
+
+  odom_msg.pose.pose.position.x = position_x_;
+  odom_msg.pose.pose.position.y = position_y_;
+  odom_msg.pose.pose.position.z = 0.0;
+  odom_msg.pose.pose.orientation = odom_quat;
+
+  odom_msg.twist.twist.linear.x = x_linear_vel_;
+  odom_msg.twist.twist.linear.y = y_linear_vel_;
+  odom_msg.twist.twist.angular.z = angular_vel_;
+
+  //  std::cerr << "linear: " << linear_speed_ << " , angular vel: " <<
+  //  angular_vel_
+  //            << " , pose: (" << position_x_ << "," << position_y_ << ","
+  //            << theta_ << ")" << std::endl;
+
+  odom_publisher_.publish(odom_msg);
 }
 
 }  // namespace westonrobot

ranger_bringup/launch/ranger_minimal.launch

@@ -5,6 +5,7 @@
     <arg name="is_ranger_mini" value="true" />
     <!-- <arg name="model_xacro" default="$(find ranger_description)/urdf/ranger_v2.xacro" /> -->
     <arg name="odom_topic_name" default="odom" />
+    <arg name="pub_odom_tf" default="false" />
 
     <include file="$(find ranger_base)/launch/ranger_mini_base.launch">
         <arg name="port_name" default="$(arg port_name)" />
@@ -16,4 +17,4 @@
         <arg name="model_xacro" default="$(arg model_xacro)" />
     </include> -->
 
-</launch>
+</launch>