From 02fe21e5ddeaf94770694976e41659ed571097fa Mon Sep 17 00:00:00 2001
From: Ruixiang Du <ruixiang.du@westonrobot.com>
Date: Wed, 8 Dec 2021 15:55:20 +0800
Subject: [PATCH] odom: fixing odom calculation for scout mini omni

---
 scout_base/include/scout_base/scout_messenger.hpp   | 4 ++--
 scout_base/launch/scout_mini_omni_base.launch       | 2 +-
 scout_bringup/launch/scout_mini_minimal.launch      | 2 +-
 scout_bringup/launch/scout_mini_omni_minimal.launch | 2 +-
 4 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/scout_base/include/scout_base/scout_messenger.hpp b/scout_base/include/scout_base/scout_messenger.hpp
index a8111dd..05a70c9 100644
--- a/scout_base/include/scout_base/scout_messenger.hpp
+++ b/scout_base/include/scout_base/scout_messenger.hpp
@@ -161,8 +161,8 @@ class ScoutMessenger {
       lateral_speed = 0;
     }
 
-    double d_x = linear_speed * std::cos(theta_) * dt;
-    double d_y = (linear_speed * std::sin(theta_) + lateral_speed) * dt;
+    double d_x = (linear_speed * std::cos(theta_) - lateral_speed * std::sin(theta_)) * dt;
+    double d_y = (linear_speed * std::sin(theta_) + lateral_speed * std::cos(theta_)) * dt;
     double d_theta = angular_speed * dt;
 
     position_x_ += d_x;
diff --git a/scout_base/launch/scout_mini_omni_base.launch b/scout_base/launch/scout_mini_omni_base.launch
index e3c1362..48d2b5d 100644
--- a/scout_base/launch/scout_mini_omni_base.launch
+++ b/scout_base/launch/scout_mini_omni_base.launch
@@ -15,7 +15,7 @@
     <arg name="simulated_robot" default="false" />
     <arg name="odom_topic_name" default="odom" />
 
-    <node name="scout_base_node" pkg="scout_base" type="scout_base_node" output="screen">
+    <node name="scout_base_node" pkg="scout_base" type="scout_base_node" output="screen" launch-prefix="valgrind">
         <param name="is_scout_mini" type="bool" value="$(arg is_scout_mini)" />
         <param name="is_omni_wheel" type="bool" value="$(arg is_omni_wheel)" />
         <param name="port_name" type="string" value="$(arg port_name)" />
diff --git a/scout_bringup/launch/scout_mini_minimal.launch b/scout_bringup/launch/scout_mini_minimal.launch
index 418634d..6cbbeaa 100644
--- a/scout_bringup/launch/scout_mini_minimal.launch
+++ b/scout_bringup/launch/scout_mini_minimal.launch
@@ -4,7 +4,7 @@
     <arg name="is_scout_mini" default="true" />
     <arg name="is_omni_wheel" default="false" />
     <arg name="simulated_robot" value="false" />
-    <arg name="model_xacro" default="$(find scout_description)/urdf/scout_mini_v2.xacro" />
+    <arg name="model_xacro" default="$(find scout_description)/urdf/scout_mini.xacro" />
     <arg name="odom_topic_name" default="odom" />
 
     <include file="$(find scout_base)/launch/scout_mini_base.launch">
diff --git a/scout_bringup/launch/scout_mini_omni_minimal.launch b/scout_bringup/launch/scout_mini_omni_minimal.launch
index 6bb83f9..62d9ed8 100644
--- a/scout_bringup/launch/scout_mini_omni_minimal.launch
+++ b/scout_bringup/launch/scout_mini_omni_minimal.launch
@@ -4,7 +4,7 @@
     <arg name="is_scout_mini" default="true" />
     <arg name="is_omni_wheel" default="true" />
     <arg name="simulated_robot" value="false" />
-    <arg name="model_xacro" default="$(find scout_description)/urdf/scout_mini_v2.xacro" />
+    <arg name="model_xacro" default="$(find scout_description)/urdf/scout_mini.xacro" />
     <!-- <arg name="model_xacro" default="$(find scout_description)/urdf/scout_mini_omni_v2.xacro" /> -->
     <arg name="odom_topic_name" default="odom" />