mirror of
https://github.com/westonrobot/scout_ros.git
synced 2026-01-12 03:15:01 +08:00
odom: fixing odom calculation for scout mini omni
This commit is contained in:
@@ -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;
|
||||
|
||||
@@ -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)" />
|
||||
|
||||
@@ -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">
|
||||
|
||||
@@ -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" />
|
||||
|
||||
|
||||
Reference in New Issue
Block a user