From 8b427fb186a1ca78d89ce683586051a86a7ca296 Mon Sep 17 00:00:00 2001
From: Michael Boulet <boulet@ll.mit.edu>
Date: Fri, 15 Jan 2016 09:39:58 -0500
Subject: [PATCH] fix bugs in odometry calc

---
 vesc_ackermann/src/vesc_to_odom.cpp | 15 +++++++++------
 vesc_driver/src/vesc_driver.cpp     |  4 ++--
 2 files changed, 11 insertions(+), 8 deletions(-)

diff --git a/vesc_ackermann/src/vesc_to_odom.cpp b/vesc_ackermann/src/vesc_to_odom.cpp
index 00d1430..71f8c73 100644
--- a/vesc_ackermann/src/vesc_to_odom.cpp
+++ b/vesc_ackermann/src/vesc_to_odom.cpp
@@ -75,6 +75,9 @@ void VescToOdom::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr&
   if (use_servo_cmd_)
     yaw_ += current_angular_velocity * dt.toSec();
 
+  // save state for next time
+  last_state_ = state;
+
   // publish odometry message
   nav_msgs::Odometry::Ptr odom(new nav_msgs::Odometry);
   odom->header.frame_id = odom_frame_;
@@ -83,17 +86,17 @@ void VescToOdom::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr&
 
   // Position
   odom->pose.pose.position.x = x_;
-  odom->pose.pose.position.x = y_;
+  odom->pose.pose.position.y = y_;
   odom->pose.pose.orientation.x = 0.0;
   odom->pose.pose.orientation.y = 0.0;
-  odom->pose.pose.orientation.z = sin(yaw_/2);
-  odom->pose.pose.orientation.w = cos(yaw_/2);
+  odom->pose.pose.orientation.z = sin(yaw_/2.0);
+  odom->pose.pose.orientation.w = cos(yaw_/2.0);
 
   // Position uncertainty
   /** @todo Think about position uncertainty, perhaps get from parameters? */
-  odom->pose.covariance[0]  = 0.1; ///< x
-  odom->pose.covariance[7]  = 0.1; ///< y
-  odom->pose.covariance[35] = 0.2; ///< yaw
+  odom->pose.covariance[0]  = 0.2; ///< x
+  odom->pose.covariance[7]  = 0.2; ///< y
+  odom->pose.covariance[35] = 0.4; ///< yaw
 
   // Velocity
   odom->twist.twist.linear.x = x_dot;
diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp
index cad169c..b03afc2 100644
--- a/vesc_driver/src/vesc_driver.cpp
+++ b/vesc_driver/src/vesc_driver.cpp
@@ -281,12 +281,12 @@ double VescDriver::CommandLimit::clip(double value)
   if (lower && value < lower) {
     ROS_INFO_THROTTLE(10, "%s command value (%f) below minimum limit (%f), clipping.",
                       name.c_str(), value, *lower);
-    value = *lower;
+    return *lower;
   }
   if (upper && value > upper) {
     ROS_INFO_THROTTLE(10, "%s command value (%f) above maximum limit (%f), clipping.",
                       name.c_str(), value, *upper);
-    value = *upper;
+    return *upper;
   }
 }